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ABSTRACT 



A pure pursuit guidance law is combined with a heading 
autopilot to provide accurate path keeping of submersible 
vehicles. The scheme is implemented and analyzed in both the 
horizontal and vertical planes. A complete stability analysis 
is performed in order to evaluate regions of stable vehicle 
operations. Numerical integrations support the analytic 
predictions. Two distinct stability boundaries are 
established. In the first, the vehicle loss of stability is 
accompanied by the generation of oscillatory motions around 
the commanded path. In the second, loss of stability occurs 
with linearly increasing path deviation. The horizontal and 
vertical plane schemes are combined with a propulsion control 
law in order to achieve path tracking of a general commanded 
route composed of several straight line segments in three 
dimensional space. 
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I INTRODUCTION 



One of the most significant functions of an underwater 
vehicle is accurate path control for transiting along 
prescribed routes in three dimensional space. The commanded 
path is usually described by a series of way points in space 
and time either by the commander or by a path planner function 
in the case of an unmanned vehicle. Without significant loss 
of generality we can assume that the commanded path can be 
approximated by straight line segments between consecutive way 
points. This assumption does not alter the important features 
of the path keeping problem since every smooth path can be 
approximated arbitrarily closely by a series of straight line 
segments. Once a desired straight line path has been 
generated, the vehicle guidance and autopilot functions are 
called upon to ensure satisfactory path keeping through the 
use of the vehicle actuators. 

One way to ensure that the vehicle goes through a 
specified sequence of way points is by using a heading 
autopilot coupled with a line of sight guidance scheme [1]. 
The scheme proved to be robust enough so that when coupled 
with an independently developed depth autopilot [2], accurate 
depth control was maintained while transiting between way 
points in the horizontal plane. The disadvantage associated 
with this technique is that the actual vehicle path between 
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two consecutive way points differ significantly from tfie 
corresponding straight line segment. 

In order to overcome this problem and achieve accurate 
path control in the presence of obstacles and underwater 
currents, a cross track error autopilot was developed for the 
horizontal [3] as well as the combined horizontal and vertical 
planes [4]. A cross track error autopilot incorporates the 
deviation of the assumed straight line path into the control 
law design. This requires the introduction of additional 
kinematic relations in the control design and, as a result, 
the controller tends to be more sensitive to actual system / 
mathematical model mismatch. 

The main drawback of a cross track error autopilot is that 
it represents a combined guidance / control scheme with no 
clear distinction between these two functions. Thus it is very 
vehicle specific and offers little flexibility in the design. 
Path control is limited to cross track error only and analysis 
of alternate schemes [5] is not possible unless the combined 
scheme is redesigned. For this reason we decide to separate 
once more the guidance and autopilot functions of the vehicle. 
An orientation controller is designed in order to provide 
accurate vehicle headings in response to guidance commands. 
The controller is, thus, based on the vehicle dynamical 
equations and Euler angle rates. A guidance scheme is used to 
provide appropriate heading commands through the kinematic 
equations of inertial position rates. A line of sight guidance 
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command law is employed as in [6] and [7]. We consider a 
reference point that is moving ahead of the vehicle at a 
constant distance on the desired straight line path. We refer 
to this distance as the lookahead distance. The commanded 
heading is then equal to the line of sight angle between the 
center of the vehicle and the lookahead point. By suitably 
selecting the lookahead distance the degree of convergence of 
the guidance law can be varied from very slow to very rapid 
onto the straight line path. 

Although the above scheme appears to be trouble free on 
the surface, a significant complication arises in the case of 
underwater vehicles. Since the actual vehicle response is 
relatively slow as dominated by the existence of important 
dynamical lags there is the possibility of instability when 
the guidance and control functions are combined. High values 
of the lookahead distance result in very slow vehicle 
response. The problem is then to evaluate these regions of 
stable and unstable vehicle response. Chapter II of this 
thesis summarizes the stability analysis results for the 
horizontal plane. In Chapter III we proceed with the analysis 
of motions under the guidance and control scheme for the 
vertical plane. It is shown that the existence of hydrostatic 
restoring moments here due to the nonzero (positive) 
metacentric height brings in an additional form of instability 
not present in the horizontal plane. Finally in Chapter IV the 
previous two guidance and control schemes for the horizontal 
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and vertical planes are combined and with a speed autopilot, 
accurate path tracking in three dimensional space is achieved. 
The main conclusion of this work is that guidance and control 
laws for underwater vehicles must be designed together even if 
they are kept separated, in order to ensure stable and 
satisfactory path keeping. All computations in this work are 
performed for the Swimmer Delivery Vehicle [8] for which a 
complete set of hydrodynamic coefficients and geometric 
properties is available. 
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II. HORIZONTAL PLANE 



In this section the vehicle equations of motion for the 
horizontal plane (x,y), the design of a heading autopilot and 
simulations and stability results are presented. 

A. EQUATIONS OF MOTION 

For the horizontal plane the mathematical model consists 
of the nonlinear sway and yaw differential equations shown 
below: 



Equations (2.1), (2.2) can be easily derived from the general 
six degrees of freedom equations for a vehicle by assuming all 
terras off the horizontal plane to be zero. The equations for 
the sway force Y and yaw moment N are presented below: 



miv+ur+x^f-yoT^) =Y 



( 2 . 1 ) 



I^r+mx^(v*ur) -my^ur=N 



( 2 . 2 ) 
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To complete the model, expressions of the inertial 



position 


rates and 


yaw rate are required. 


These are the 


kinematic 


equations : 










^=r 


(2.3) 






x=ucosi|;- vsim|; 


(2.4) 






y = u s i rnjj - vc o s 


(2.5) 



B . CONTROL LAW 

It is more convenient for the design of a linear state 
space heading controller to represent the above equations 
(2.1), (2.2), (2.3) in the following form (with Yq=0): 



i|f=r 


(2.6) 


v=a^.^uv+a^ 2 ^r+b^u^b+d^iv, r) 


(2.7) 


r=a 2 -^uv+a 22 ur+b 2 u^() +d^ ( v, r) 


(2.8) 



where : 






^11 = ^ i Y^~ imx^-Yr) NJ 
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<321 = ^ [ irrt-Y^)N^- imx^-N^) Yj 
322 = -^ [ im-Y^) i-mxQ+Nj.) -(mx^-N^) (-rn+Y^) ] 
b, = ^[iI^-N,) Y,-{mx^-Y,)N^] 
b^ = l[{m-Y^) Y,-(mx^-N^) Y,] 
d,{v, r) =-^^pC^^[{I,-N,) I,^Y,I,] 
d,{v,r) =-llpC^^[{w-Y^) 

I,=f [h(V (v+^r) |(v+^r) |]d^ 

I,=f [Ji(^) (v+^r) l(v+^r) |^]d^ 

The nonlinear terms d^( v, r ) ,d^(u, r) are small and can be 
neglected for control law design. They are kept, however, in 
all numerical simulations that follow. 

1. ZERO YAW ANGLE 

When the commanded yaw angle of the vehicle is zero the 
control law has the following form: 
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b=k^\^ +k2V+k^r 



( 2 . 9 ) 



where k^,k2/k3 are computed so the system will have the desired 
dynamics. The closed loop characteristic equation has the 
following form: 

+52 A, +3j =0 (2.10) 



where : 



a^=a.^^u+a22u+b.^u^k2+b2u^kj 

^3“ (-^2^11 ”-^1^21 ^ k -^ 

The characteristic equation is specified in the following 
way. It can be chosen to satisfy the minimum ITAE criterion 
where it assumes the form: 

A^^a2A.^+a2'^+<^3=0 (2.11) 



where : 



. 7 5 o>o 



tt2=2 . 15(0 q 



a 



3 



= (0 



3 

0 
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and represents the dimensionless settling time for the 
system. Equating the coefficients of equation (2.10) with the 
desired equation (2.11) and after some algebra we find: 



Selecting a value for t^^ according to the ITAE criterion, 
dictates complex conjugate dominant poles with oscillatory 
transient response. It was found that other poles selections 
(for example real negative) do not change significantly the 
nature of the results and the stability boundaries that are 
presented later. 

2. NON ZERO YAW ANGLE 

If the commanded yaw angle is non zero and equal to ijr ^ 
then the control law (2.9) is simply modified to: 






( 2 . 12 ) 



^2 ^^1^22~^2^12'> U^+k;^ ^^2^11~^1^2l'^ U^=a2+b2U^k^ (2.13) 






(2.14) 



6=k^ (ilf-vlJc) +k.v+k^T 



(2.15) 
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Figure 1. Horizontal plane geometry 
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No feedforward term is necessary in (2.15) since no rudder 
angle is required to keep the vehicle to a constant non zero 
heading angle at steady state. 

C. GUIDANCE 

The heading autopilot that was designed in the previous 
section is called upon now to provide vehicle path in the 
sense of passing through a series of way points in the 
horizontal plane. In order to achieve it without changing the 
previously designed heading autopilot we have to couple it 
with a suitable navigation scheme such as line of sight 
guidance . 

The simplest such guidance law is a pure pursuit 
navigation which is accomplished as follows. The autopilot 
attempts to point the longitudinal axis of the vehicle towards 
a point D which is located ahead to the vehicle on the nominal 
straight line path at a fixed distance x^ as shown in Figure 
1. This target distance x^^ to as the visibility, lookahead, or 
preview distance. The line of sight angle a is defined by: 

tano = -^ (2.16) 

Pure pursuit navigation then corresponds to taking: 

= o (2.17) 
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as the coitunanded heading angle in the control law (2.15). 

It can be seen now that the commanded vehicle heading 
angle is not constant but it is function of the vehicle 
position y. This introduces the lateral deviation equation 

(2.5) into the problem, and since the control law was based on 
equations (2.6), (2.7) and (2.8) only, stability of the 
combined autopilot-guidance scheme is no longer guaranteed. 
Therefore, we need to develop conditions which will guarantee 
stability and ensure satisfactory path keeping. 

D. STABILITY 

The complete system is given by the differential equations 

(2.6) , (2.7), (2.8), the control law (2.15), and the guidance 
equations ( 2 . 16 ) , ( 2 . 17 ) . The trivial equilibrium state 
corresponding to a straight line motion is characterized by: 

\{r = v=r=y=0 



Linearization of the state equations gives the following 
linear system: 

X=AX 



where the complete state vector is: 

X= [i|f , V, r,y] 
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Local stability properties are established by the eigenvalues 



of [A] The 


characteristic equation is found to be: 




AX^-^BX^ + CX^^Dk^■E=0 (2.18) 



where : 


A=1 

B=-B,-C, 

C= +B^C2’-C^B2 ~^2 




D=-C^D2 C 2 ” IID 2 



and 


A2=b^u^k^ 

Bi=a-^iU+b.^u^k2 

Cj^=ai2U+jbjU^ic3 
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( 2 . 19 ) 









Loss of stability occurs when: 

BCD-B^E-AD^=0 



Equation (2.19) is derived from Rooth ' s criterion for (2.18), 
and it corresponds to a pair of complex conjugate roots 
crossing the imaginary axis. After some algebra equation 
(2.19) is simplified to: 

aj^x^+a2X^+a^=0 ( 2 . 20 ) 



where : 



a- =a,a2-«3 



^2“ 






■^2*^11 -^1^: 



-a?u 
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(b^a^^-b^a2^)^u 



The positive root of equation (2.20) determines the 
critical value of x^ for stability. For every x^ > the 

system is stable which means that the vehicle will follow the 
path. In the opposite case where x^ < x^ critical system 
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becomes unstable and the motion of the vehicle becomes 



oscillatory as a result of a complex conjugate pair of 
eigenvalues with positive real parts. 

Results for the dimensionless critical visibility versus 
settling time t^^ are presented in Figure 2. These results are 
independent of the forward speed since gains k^,k 2 /kj are 
functions of u. It can be seen from Figure 2 that for higher 
t^ (softer controller) higher lookahead distance is required 
in order for the system to remain stable. It is obvious that 
very high values of x^^ correspond to a very slow navigator 
with a loss in speed of response and navigational accuracy. 
The results of this section establish analytically the minimum 
required lookahead distance that is required for stability 
based on linear approximations. 

It should be mentioned that all results in this work are 
presented in dimensionless form unless otherwise mentioned. 
Nondimensionalizations are performed by using the vehicle 
length and the vehicle forward speed. 
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E. SIMULATIONS 

Numerical simulations confirm the results of the stability- 
analysis of Figure 2. The simulated lateral distance y (in 
vehicle lengths) versus time t (in dimensionless seconds) is 
shown in Figure 3 for two cases. The nominal straight line 
path is y=0 Case 1 is located in Region 1 of Figure 2 and it 
can be seen that the vehicle response is unstable. Case 2 
corresponds to a stable (t^,x^) combination and the vehicle 
converges to the desired path. 
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0.3 



1 : t„ = 4, Xd = 

2: tn = 4, Xd = 2 



O 




0.0 2.0 4.0 6.0 8.0 10.0 

t 



Figure 3. Stable and unstable numerical simulations 
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III. VERTICAL PLANE 



In this section the vehicle equations of motion for the 
vertical plane (x,z), the design of a vertical heading 
autopilot and simulations and stability results are presented. 

A. EQUATIONS OF MOTION 

Restricting our attention to the vertical plane the 
mathematical model consists of the nonlinear heave and pitch 
differential equations shown below: 

/ . . (3.1) 

m{w-uq-XQq-ZQq^) -Z 

lyq-mxQ iw-uq) +mz^wq=M (3.2) 

where only vertical plane related terms have been kept. The 
heave force Z and pitch moment M are written as : 

Z=Z^q+{Z^w+ZgUq) +Z^uw--^ jc^b(x) -^j^^^^dx+{W-B)cosQ + 

M=M^q+ {M^w+M^uq) +M^uw+-^ xdx- (x^W-x^B) cos6 

- ( ZqW-ZqB) sin0 + u^ 



In the above equations is the vehicle weight, B the buoyancy, 
(Xg,Zg) the coordinates of the center of gravity, and (X 3 ,Zg) 
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the coordinates of the center of buoyancy. Also, provision for ’ 
two sets of control surfaces (stern and bow planes) is made. 
The kinematic equations are: 



x=ucos0+wsin6 


( 3 . 3 ) 


i=-usin6+wcos0 


( 3 . 4 ) 


0=g 


( 3 . 5 ) 



B . CONTROL LAW 

The linearized state space form of equations (3.1), (3.2) 
and (3.5) is used for vertical plane heading control: 

w=a.^.^uw+a.^2^g+a.^^d+b^.^u^dg+b.^2^^^b ( 3 - 6 ) 

g=a2iUW+a22ug-*-a22^+b2iU^b^+b22u'^^t (3 * 7 ) 

6=g 



where : 



D^=(ni-z^) ily-M^) - imx^+Z^) (n}x^+My) 



^11 = ^ [ Z,*{Tnx^^Z^)Mj 



■ 3 i 2 = -^ [ (m+Zg) + intx^+Z^) ] 

^ \r 
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a,3 = --^ [ (Zg-^s) W] 

^11 = ^ [ (^<3+V 

-^12 = -^ [ ( (^g+ 2^) WfiJ 
<321 = -^ [ (m-2^) (/nxG+W^) Zj 

-322=-^ t (^-Z^) iMg-m) + imx^+M^) im+Z^) ] 

‘^ 23 “””^ [ (/n-Z^^) [Zq-Zq) W] 

^21 = ^ [ (/n-Z^)i^,,^ (/nx^^M^) Z,,] 

^22 = -^ [ imx^+M^) ZjJ 



In these W=B and Xg=Xg have been assumed. Considering that the 
effect of the bow and the stern planes is the same we have: 

6,=6 

6^=-6 
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so 



i?i b 



12 



•^2 -^21 -^22 



From the above the final form of the equations of motion 
is : 



II 

<D 




w=a^y^uw+a^2^q+a^2^+b^u^b 


(3.8) 


g=a2iUW+a22ug+a230+jb2U^6 


(3.9) 



1. ZERO PITCH ANGLE 

When the commanded direction of the underwater vehicle is 
horizontal the control law has the following form: 

6=k^Q+k2W+k^g (3.10) 



where k^,k 2 ,k .3 are calculated below. From the system of the 
three differential equations (3.5), (3.8), (3.9) the closed loop 
characteristic equation has the following form: 

+ 5^ =0 (^* A 1 ) 



where ; 
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u-jb^ u ^ic2 -322 u -jb>2 ^-^3 

^ 2 =^ 11 ^ 22 ^'^ 

^ 3 “^ 13 ^ 21^”^13 ^2 ^ ^^ 2 -^ 1<321 ^ ^-^1 + A ■^< 323 ‘fel ^ ^-^2 
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Figure 4. Vertical plane geometry: Horizontal commanded path 
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The desired characteristic polynomial according to the ITAE 
criterion is: 



X^+aj^X^+a2A+a3=0 



( 3 . 12 ) 



where : 



a 



a 



3 = 1 .75u) 



2=2 . 15(0 




^0 = 



lOu 



0 



2 

0 



and t^ represents the dimensionless settling time for the 
vertical plane autopilot. Equating the coefficients of 
equation (3.11) with equation (3.12) we get: 



b^u^k 2 +b 2 u'^k^ = -a^- ( 333 + 322 ) u 



( 3 . 13 ) 



(^1^22 -^2 -212) ^-^ 2 ^ (^ 23 i 1 -^ 1 ' 22 i) ^ ’ ic, = , +1^2 2/C3 



(■^2‘^H -^1^21) ^^■^1'*’ ^‘^23'^1 ^ 12 ^ 2 ^ ^^^2 
tt3+ (3 i332i-3ii323) U 



( 3 . 15 ) 



To simplify notations, equations ( 3 . 13 ) , ( 3 . 14 ) and (3.15) are 



written as : 
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(3.16) 



=D^ 



B -^ k ^ ■‘■■^ 2-^2 '*'^ 3^3 ~^2 



(3.17) 



Cl k^ + C 2 k2 -D^ 



(3.18) 



where : 

A 2= b ^ u ^ 

A 2= b 2 U ^ 

Br = b 2 U ‘^ 

■®2 ~ ^•^ 1 ^ 22 ~'^ 2 ‘^ 12 ^ 

■®3 “ ^ -^2^11 ”-^1^21 ^ ^ 

Ci= (i32aii-jbia2i) 

^2~ (^ 23-^1 ”‘^ 13 -^ 2 ^ 

■^2“®^ 2 "*^^2 3'*’ ^^12‘^21~^11^22^ 

£> 3 =a 3 + (513321-311323) U 

From the above system of equations (3.16), (3.17), (3.18) we 
can find expressions for the gains ki,k 2 ,kj 
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(3.19) 






D3 02k 2 



c. 



^2 = 



+ C3S3D3 -D2 C3A3 
A3B1C2 +C3B3A2 -C3A3S2 



(3.20) 



_ £>3 ^2-^2 



(3.21) 



2. NON ZERO PITCH ANGLE 

When the commanded pitch angle of the vehicle is not equal 
to zero, we have: 

B=ay^Q' (3.22) 



where : 



Then 



a^ is the commanded pitch angle 
0 ' is the deviation from the commanded angle 



sin0=sina^,cos0'+cosa^,sin0^=sina^,+9'cosa^ 



for small deviations 0’. The system of equations of motion 
(3.5), (3.8), (3.9) takes the form: 

0'=g (3.23) 

w=a^^uw+a^2^g+a^2^^BaJd^+b^u^b +aj_2sina^ (3.24) 
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Figure 5 




Vertical plane geometry: Inclined commanded 



path 
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g=a2;^uw+a22UQ+3.22COsaJd'+b2U^b +a23sina^. 



(3.25) 



The control law now takes the form: 

b=k^iQ-a^.) +k2W+k2Q+k^ (3.26) 

where k^,k 2 /kj can be calculated with the some procedure as 
before, and the feedforward gain k^ is calculated from the 
desired steady state accuracy. At steady state we have: 

g=0 



0=a 



V 



e'=o 

so that the system of the equations of motion 
(3.23) , (3.24) , (3.25) yields: 

aiiUw+i:>iU“6 +ai3Sina^,=0 (3.27) 

a2iUw+b2ii^6 +a23sina^,=0 (3.28) 

Equations ( 3 . 27 ) , ( 3 . 28 ) can be solved for the steady state 
values of 6 and w, and by substitution into equation (3.26), 
after some calculations k^ is found to be: 
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(3.29) 



[, __ ^13 ( ‘^ 21'*’ -^2 ^-^ 2 ^ ^2 3 ‘*'■^1 ^^ 2 ^ 

■^4 “ T ^ ^ r Slila, 



(■^ 1^21 -^ 2 ^ 11 ^ 



Note that if 3^=0 or Zg=Ze then k^=0. 

C. GUIDANCE LAW 

A similar to the horizontal plane case guidance law can be 
used here to allow path keeping in the vertical plane. To the 
previous system of differential equations (3.1), (3.2), (3.5) 
one more equation is added, the kinematic equation (3.4). The 
new system is now going to be examined for two different 
cases . 

a) Horizontal path (no change in depth) 

b) Inclined path (change in depth) 

1. HORIZONTAL PATH 

In this case where the commanded depth remains the same 
the control law is: 

6 =ic^ (9-0^) +ic2W+JCjg (3.30) 

where 6^ is the commanded line of sight (pitch angle) 

e^=tan-^-^ (3.31) 
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where k^,k. 2 ,k .3 are already known from the previous section, and 
Xj is the visibility distance similar to the horizontal case, 
shown in Figure 4. 

2 . INCLINED PATH 

Here the commanded depth changes linearly so that the 
angle 6 is given by: 



where k^,k 2 ,kj,k^ are the same as previously determined. 

The k,^ term exists here because an angle 6 0 has to remain 
when the underwater vehicle changes depth to equalize the 
restoring moment due to the pitch angle. The commanded pitch 
angle is: 



where z' is the cross track error off the inclined path as 
shown in Figure 5. 

D. STABILITY 

The complete system is given by the equations of motion 
(3.1), (3.2), (3.4), (3.5), the control law (3.30), and the 

guidance law (3.31). Horizontal motion at the commanded depth 
is characterized by: 



6=k^ +k2W+k^q+k^ 



(3.32) 




(3.33) 



0=w=g=z=O 
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Linearization of the above equations produces the linear 
system: 

X=AX 

where : 

X= [0, w, q, z] 



and 



0 


0 


1 


0 




a^^u+b^u^K2 








a^^u+b^u'^ K 2 




-jb, u — - 
^d 


-U 


1 


0 


0 



where : 



^GB 



(3.35) 



is the metacentric height. Stability properties of the 
straight line motion are established by the eigenvalues of 
matrix [A]. It should be mentioned that from now until the end 
of this chapter a^j, ajj have been redefined to show explicitly 
the metacentric height Z^g. 

A program is written to compute the eigenvalues of matrix 
(3.34) over a range of (t^,x^) values, and detect whether one 
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or more eigenvalues become unstable. Typical results are shown 
in Figure 6 for u=5 ft/sec and Zgg=0.1. 

1. REGIONS OF STABILITY 

It can be seen that the stability boundary of Figure 6 
separates the parameter space (x^,ty) into three regions: 

1: Unstable region, one pair of complex conjugate 

eigenvalues of [A] has positive real parts. 

2: Stable region, all eigevalues of [A] have negative 
real parts. 

3: Unstable region, one real positive eigenvalue of 

[A]. 

Obviously, stable vehicle response is not possible unless the 
parameters (x^,t^) are chosen in region 2. 
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12.0 16.0 



1: One pair of complex conjugate eigevalues with 
positive real parts. 

2: Region of stability. 

3: One real positive eigenvalue. 




J<d 



J 

Figure 6. Regions of stability for u=5 ft/sec and ZgB=0.1 ft 
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2 . 



SIMULATIONS 



Before proceeding further with the stability analysis, 
numerical integrations are first performed in order to examine 
the response of the vehicle in each of the above three regions 
of stability of Figure 6. The same parameters u=5 ft/sec and 
Zgg=0.1 ft are used. Simulations for the pitch angle 0 and the 
commanded line of sight angle 0^ for the case t^=5 and x^j=4 are 
shown in Figure 7 . This corresponds to region 2 of Figure 6 
which is the region of stability. The simulation results show 
that the actual vehicle pitch angle approaches the commanded 
angle, after some oscillations, and the depth reaches its 
commanded value at zero as predicted. 

When the visibility distance is x^=0.4 with the same t^, 
the vehicle moves into the unstable region 1 of Figure 6. The 
simulated response is shown in Figure 8 where oscillatory 
characteristics are exhibited. If we keep the same value for 
X|j=4 and we change the controller time constant t^=15 we enter 
the unstable region 3 of Figure 6. The simulated vehicle 
response is shown in Figure 9 where it appears that 0 and 0^. 
diverge and they both reach nonzero steady state values. As a 
result the vehicle depth is now a linear function of time, 
without ever stabilizing. These results require a more 
detailed analysis of the regions of stability of the 
controller / guidance combination. 
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Zg3 = 0.1 (ft) 
u = 5 (ft/sec) 
= 5 
Xd = 4 




0 . 0 



5 . 0 



10. 0 

t 



15 . 0 



20. 0 



Figure 7. Numerical simulations in region 2 
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degrees 



ZcB = 0.1 (ft) 

u = 5 (ft/sec) 
= 5 

Xd = 0.4 

O 




t 



Figure 8. Numerical simulations in region 1 
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Z-3 = 0.1 (ft) 
u = 5 (ft/sec) 
t, = 15 
X, = 4 



O 




t 



Figure 9. Niomerical simulation in region 3 
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Xd 



Figure 10. Regions of stability for z„=0 and for any speed 
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E. ANALYSIS 



The stability properties of the system are characterized 
by the eigenvalues of the linearized matrix [A], given by 
equation (3.34). The characteristic equation of [A] has the 
form : 

AX^+5A^+CA,2+£)A,+f:=0 (3.36) 



where : 



A = 1 



B=a^ 

D= a 3 + ( ^2 ^22 ) U ' ic, ^ -^2 u 

According to Routh’s criterion (3.36) has one pair of complex 
conjugate roots crossing the imaginary axis when: 

BCD-AD^-B^E=0 (3.37) 



After some algebra , equation (3.37) can be written as : 
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Oj x^+ [d^ (tt;^a2-a3) +a3Cia3-a3d3-aieJ (3.38) 

x^+di (aiCi-d^) =0 



where : 

Ci=^2-^i 

d^ = ( -B^-A^u) 

e^= ( -C2+C3U) 

and A 2 , A 3 , B 2 C 3 , C 2 were defined previously following equations 
( 3 . 16 ) , ( 3 . 17 ) and (3.18). 

The positive root of equation (3.38) provides the critical 
value of for stability. This produces the curve separating 
the regions 1 and 2 of Figure 6 . As the (x^,t^) combinations 
cross into region 1 , the response of the system becomes 
oscillatory as a result of the pair of complex conjugate 
eigenvalues with positive real part. This explains the 
simulations observed in Figures 7 and 8 . 

A different kind of instability occurs when one real root 
of (3.36) crosses zero. For this to happen the condition is: 

E=0 ( 3 . 39 ) 



and using the previous definition of E, this happens when 
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(3.40) 



k ^=0 



Equations (3.40) and (3.19) yield 




(3.41) 



Equations ( 3 . 41 ) , ( 3 . 20 ) define then the critical condition for 
stability. In our case this can be simplified as follows. The 
expression for is reproduced here. 

Since we have assumed that bow and stern planes have the same 
strength 

^6s~^6b^^ 

and substituting the expressions for ajj, b^ , a^^, bj we can find 
that 

C2=0 (3.43) 

Equations (3.43) and (3.41) then require that 
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and using the definition for Dj we get 



OC3 + (<ai 3 C? 2 i ^ 11 ^ 22 '^ 



or 









and we can find the critical value of t, as 



lOu 



^czi tical 



-^[(^ 11*323 ^ 13 ^ 21 ^ 



(3.45) 



Condition (3.45) shows that the critical value of t^ is 
independent of which is demonstrated in Figure 6 as a 
straight line parallel to the axis. Furthermore, the other 
stability curve, equation (3.38), intersects the t^ axis at 
X|j=0 when k^=0 which is the same condition as (3.45). This 
means that the stability conditions (3.38) and (3.45) separate 
the (X|j,t^) parameter space into three regions of stability, 
as shown in Figure 6. 

Results of the stability regions for Zqb= 0 are shown in 
Figure 10. These are independent of the forward speed u just 
as in the horizontal plane case. It should be mentioned that 



for ZgB"®' ^vcriticai ^ therefore , region 3 of figure *6 ■ 
never appears . 

For Zgg > 0, the stability regions depend heavily on the 
forward speed u. This is demonstrated in Figure 11 for Zgg=0.1 
(ft) and various values of u in (ft/sec). As the speed is 
decreased the critical value of t^ from (3.45) also decreases 
with the effect of reducing region 1 and enlarging region 2 
and 3 . 

The effect of varying the metacentric height z^g while 
keeping u constant is evaluated in Figure 12 for u=2 . Similar 
conclusions can be drawn for this case as previously. 

The critical value of t^ as given by (3.45) is shown in 
Figure 13 for different values of the forward speed u and the 
metacentric height z^g. The surface shown in the figure 
separates the stability regions 2 and 3. 

The final task of this section is to explain the 
simulations observed in Figure 9 when the vehicle operates in 
region 3 . 
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Xd 



Figure 12. Regions of stability for u = 2 ft/sec 
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F. STEADY STATE SOLUTIONS 

It was shown in the previous section that transition 
between Regions 2 and 3 is associated with a real eigenvalue 
crossing zero. Usually when such a loss of stability occurs 
and the primary equilibrium solution becomes unstable, 
additional stable equilibrium solutions appear. To evaluate 
these new steady state solutions we consider the complete 
system given by equations (3.4), (3.5), (3.6), (3.7), (3.30), 
and (3.31). At steady state the time derivatives vanish and we 
get 

g=0 (3.46) 

w= — sin0 (3.47) 

6 = 3 . : ^ sine (3.48) 



Substituting equations ( 3 . 46 ) , ( 3 . 47 ) , and (3.48) into equation 
(3.4) we get : 

( -u+ — cos0) sin0=O (3.49) 

Equation (3.49) may accept besides the normal solution 0=0, 
another solution given by: 
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( 3 . 50 ) 



COS0 = — ^ u = 



(i>2<3ii-jbi<32i) 

”-^2^13 ^ ^GB 



Equation (3.50) is valid provided cos0 < 1 which means: 



If (3.51) is satisfied the equilibrium angle 0 can be 
determined from (3.50) provided: 



where is the maximum dive plane angle typically set at 
0.4 radians. 

In our case conditions (3.51) and (3.52) are not 
satisfied, which means that the non zero equilibrium pitch 
angle cannot be computed from (3.50). Furthermore z ^ 0 at 
steady state, which means that z=constant. Therefore, z is 
linearly increasing with time, and 



^ 2 ^ ^•^ 1*^23 -^ 2 ^ 13 ^ ^GB 




( 3 . 51 ) 




sat 



( 3 . 52 ) 




( 3 . 53 ) 
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Substituting equations (3.46), (3.47), (3.48), and (3.53) into' 
the control law (3.30) and (3.31) we can find the equation for 
the unknown steady state pitch angle. 

(Dj-ttj) sin0=JCj^Cj^ ^ +iC2C2sin0 (3.54) 

If we call 0 - n/2 = 0', equation (3.54) reduces to: 

ia^-k^C^) cosd'=0 (3.55) 

It can now be seen that equation (3.55) has a solution when 
crosses zero which is the same condition for transition 
between regions (2) and (3) found in the previous section. The 
steady state solution is then computed from (3.55) if: 

3 “j sin0^6^3, (3.56) 



or from: 



sin0 = 



^ 3-«3 



sat 



(3.57) 



Otherwise . 

Results for the steady values of 0 and 5 are presented in 
Figures 14 and 15 versus z^g for u=5 and t^=15. 
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Solid lines correspond to stable and dashed lines to unstable 
equilibrium positions. It can be seen that the simulation 
results for =0.1 of Figure 9 are verified. 

The steady state pitch angle 6=0 loses its stability at 
Zgg=0.07 and begins to increase together with the dive plane 
angle 5. This is up to Zgg=0.12 where 5 reaches its maximum 
value. For increasing Z^g beyond this point, the pitch angle 
6 begins to decrease since 5 remains constant. These results 
are for fixed t^ and u. Results for different values of the 
controller settling time t^ and vehicle speed u are shown in 
Figures 16 and 17 respectively. 
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u = 5 (ft/sec) 
= 15 
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Zgb ( f t ) 



Figure 14. Steady state pitch angle 6 versus 
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tv = 15 
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Figure 15. Steady state dive plane angle 5 versus z^a 
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Figure 16! Steady state 0 versus z^b for several values of t, 




54 



= 15 



O 




^GB (ft) 



Figure 17. Steady state 0 versus z^b for several values of u 
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IV THREE DIMENSIONAL GUIDANCE CONTROL 



The horizontal and vertical plane guidance and control 
laws that were developed in the previous two chapters are 
combined here to provide accurate path keeping in three 
dimensional space. The other requirement is that the forward 
speed along the path should be constant and equal to a 
commanded value. This will enable path tracking instead of 
simply path keeping. 

A. PROPULSION CONTROL 

Just as the horizontal (vertical) plane path control 
design was based on the linearized lateral (vertical) 
equations of motion, the propulsion control law will be based 
on the linearized form of the surge equation only. The surge 
equation is: 

mu=X^u+X^^w^ + uw{X„(,^-X^^(,J b-^u^ b^+Cp^ia^n'^-u^) (4.1) 



where : 



a = 



^nax 

•^ax 



(4.2) 



n is the propeller revolutions, and 6 the dive plane angle. 
Only w and 6 terms remain in equation (4.1) because only these 
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terms are nonzero at steady state for a constant commanded 
dive or rise angle. A propulsion control law is introduced of 
the form: 

n=no+k„{u-u^) (4.3) 



The feedback gain is computed from stability 

requirements whereas the feedforward term nQ is computed from 
steady state accuracy. When n=nQ the forward speed u must 
equal the commanded speed u^.. Therefore, (4.1) becomes: 

f (u^) +C^^a^r!o =0 (4.4) 



where we defined 



f(u) =X^w^+uwiX^f, -X, 



w6 






(4.5) 



The terms w and 6 are given as functions of u^. and the 
commanded pitch angle a^ by 



w= 



( 511^2 ~a2j^bi) Uj, 



(4.6) 



(4.7) 



Solving (4.4) for we get 
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( 4 . 8 ) 




f(uj 

^0 



This term Hq guarantees the required steady state accuracy. To 
evaluate we substitute (4.3) and (4.8) into (4.1) and we 
get : 



(m-X^) u-2Cj^a^n^k^{u-u^) =0 



( 4 . 9 ) 



The characteristic equation of (4.9) is 



2c^yn^k„ 

s =0 

m-X„ 



The desired characteristic equation is 



s+o>o=0, . . a>o=- 



lOu, 



( 4 . 10 ) 



( 4 . 11 ) 



where t^ is the desired dimensionless settling time for the 
speed control. Comparing (4.10) with (4.11) we can solve for 
the control gain. 



5u^im-X^) 



( 4 . 12 ) 
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with the choice of gains (4.12) and (4.8), the propulsion 
control law (4.3) is complete. 



B. THREE DIMENSIONAL PATH KEEPING 

Suppose the commanded path is a general straight line in 
three dimensions, from point 0 to point F as shown in Figure 
18. The vehicle position is at point A. With respect to the 
inertial coordinate frame (x,y,z) the commanded path is 
characterized with the two angles a^ and a^ as shown in the 
Figure. In order to achieve the commanded path, a coordinate 
frame rotation by an angle a^ is performed first as shown in 
Figure 19. The necessary geometric relations are: 



a,=tan-i:^ 



(4.13) 



>^'= (y-Yo') sina^+ (x-x^) cosa^. (4.14) 

y'= iy-y^) cosa^- {x-xj sina^ (4.15) 



The rudder control law is then of the form: 

6 (ijf-a^-o^) +ic2 v+iCjX (4.16) 



where the line of sight angle for horizontal plane control 
is defined by: 
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(4.17) 



is the lookahead distance determined according to the 
stability analysis of Chapter II, and ^ the 
horizontal plane control gains from Chapter II. 

Another rotation by an angle a^ is conducted next as shown 
in Figure 20. The geometric relations here are: 



where the line of sight angle for vertical plane control is 
defined by: 




(4.18) 



x’ (Vf-Vo) sina,^+ (x^-x^) cosa^j 



(4.19) 



x"=- ( z-zj sina^+x'cosa^ 



(4.20) 



z'= ( z-z^) cosa^+x'sinUy 



(4.21) 



The dive plane control law is: 



d=k^ (0-a^-o^) 



(4.22) 



z ^ 

tano = 
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is the lookahead distance determined according to the 
stability analysis of Chapter III, and , k 2 , k^, k^ are the 
vertical plane control gains as computed in Chapter III. The 
existence of two distinct distances x^^, x^^ is for maximum 
flexibility in the design and to allow for the possibly 
different stability conditions for horizontal and vertical 
plane, as analyzed in the previous two chapters. 

Results are presented for a typical three dimensional 
commanded route that consists of the following way points (x, 
y, z) = (20, 0, 5), (40, 5, 5), ( 60, -5, -3), (100, 0, -5) 
vehicle lengths with individual straight line paths connecting 
them. Switch from one to the next straight line path was 
initiated when the vehicle position, measured along the 
current commanded path, was within a specified target distance 
(TD) from the way point. Parameters used for the simulation 
were the following: t^=7, t^=5, Zg=0.1, t,^=0.2, x^j^=3, x^^=2.5 
commanded speeds u=(4, 4, 5, 5) for the four straight line 
segments respectively, and TD=1. Simulation results are 
presented in Figure 21 through 25. It can be seen from Figures 
21 that accurate path control is maintained in both the 
horizontal and vertical planes. Speed control is also very 
accurate, see Figure 22, despite the course changes and 
nonzero dive and rise angles. The speed controller revolutions 
per minute are shown in Figure 23, where the maximum 
saturation limit is set 500 rpm. Rudder response is shown in 
Figure 25 where the steady state nonzero values occur during 
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a nonzero commanded pitch angle. Comparing Figures 24 and 25 
with 22, it can be observed that the vehicle slows down 
momentarily when the control surfaces become active, a 
situation which is quickly corrected by the speed controller. 
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Figure 18. Coordinate transformation for 3-D path keeping 
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Figure 20. Vertical plane rotation 
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Figure 22 . Time history of vehicle speed u 
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Figure 23. Time history of propeller revolutions per minute 
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Figure 24~ Time history of rudder angle 
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Figure 25. Time history of dive plane angle 
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CONCLUSIONS AND RECOMMENDATIONS 



The main conclusions and contributions of this work can be 
summarized as follows: 

1. Pursuit guidance law and decoupled horizontal and 
vertical plane orientation controllers were shown to provide 
accurate vehicle path keeping in three dimensions. 

2. The scheme proved to be robust enough so that it could 
handle the nonlinear coupling between speed response, and 
horizontal and vertical plane motions without performance 
degradation . 

3. It was shown that the guidance and control schemes must 
be designed together in order to avoid loss of stability or 
excessive oscillatory response. 

4. Analytic conditions for stability were derived. The 
conditions were expressed explicitly in terms of the vehicle 
hydrodynamic characteristics and the guidance and control law 
design specifications. 

5. An extensive study of the mechanism of loss of 
stability was undertaken for the vertical plane motions. Two 
distinct possibilities were discovered and analyzed. In the 
first one pair of complex conjugate eigenvalues crosses the 
imaginary axis and results in an oscillatory vehicle behavior 
around the commanded path. In the second , one real eigenvalue 
crosses zero and the vehicle drifts off to a steady state path 
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with its deviation from the commanded path linearly increasing 
with time. This new path was computed and explicit conditions 
to avoid such a undesirable situation were given. 

Some recommendations for further research include the 
following: 

1. Comparisons from the point of view of path keeping 
response under physical system / mathematical model mismatch. 

2. State estimators must be included in the analysis to 
evaluate performance under partial state knowledge and sensor 
noise . 
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APPENDIX A 



C PROGRAM SIM_3D.F0R 

C 

C FOTIS A. PAPOULIAS/ANGELLOS G. PAPASOTIRIOU 

C NAVAL POSTGRADUATE SCHOOL 

C AUGUST 1991 

C 

C VEHICLE THREE DIMENSIONAL PATH KEEPING 

C HEADING AUTOPILOTS 

C PURE PURSUIT NAVIGATION 

C SIMULTANEOUS RUDDER/DIVE PLANE SWITCHINGS 

C 

C DECLARATIONS 

C 

REAL L,MASS, IX, lY, IZ, IXZ, lYZ, IXY 
REAL K1H,K2H,K3H,K1V,K2V,K3V,K4V,KN 

REAL KPDOT , KRDOT , KPQ , KQR , KVDOT , KP , KR , KVQ , KWP , KWR , KV , KVW , 
& KPN,KDB 

REAL MQDOT , MPP , MPR , MRR , MWDOT , MQ , MVP , MVR , MW , MW , 

& MDS , MDB , NDRB 

REAL NPDOT , NRDOT , NPQ , NQR , NVDOT , NP , NR , NVQ , NWP , NWR , 

& NV,NVW,NDRS 

REAL MM(6, 6) , INDX( 100) 

DIMENSION X(9) , BR( 9 ) , HH ( 9 ) , VECHl ( 9 ) ,VECH2(9) ,XMMINV(6, 6) 
DIMENSION VECV1(9) ,VECV2(9) , F ( 12 ) , FP ( 6 ) , DISV( 100) 
DIMENSION XDES(IOO) , YDES(IOO) ,ZDES(100) ,UDES(I00) , 

& DISH(IOO) 



GEOMETRIC PROPERTIES 


WEIGHT=12000.0 


IX 


= 1760.0 


lY 


= 9450.0 


IZ 


=10700.0 


IXY 


= 0.0 


lYZ 


= 0.0 


IXZ 


= 0.0 


L 


= 17.425 


RHO 


= 1.94 


G 


= 32.2 


XG 


= 0.0 


YG 


= 0.0 


XB 


= 0.0 


YB 


= 0.0 


ZB 


= 0.0 
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AO = 2.0 

CDO = 0.0057 

MASS =WEIGHT/G 
BOY =WEIGHT 
RPMMAX= 500.0 
RPMMIN= -500.0 
UMAX = 6.0 

UMIN = 0.1 

ALPHA =UMAX/RPMMAX 

SURGE HYDRODYNAMIC COEFFICIENTS 

XPP = 7 .030E-03*0.5*RHO*L**4 
XQQ =-1.470E-02*0.5*RHO*L**4 
XRR = 4 . 010E-03*0 .5*RHO*L**4 
XPR = 7 . 640E-04*0.5*RHO*L**4 
XUDOT =-7.580E-03*0.5*RHO*L**3 
XWQ =-l . 920E-01*0.5*RHO*L**3 
XVP =-3.240E-03*0.5*RHO*L**3 
XVR = 1 . 890E-02*0 .5*RHO*L**3 
XQDS = 2 . 610E-02*0.5*RHO*L**3 
XQDB =-2 . 600E-03*0 .5*RHO*L**3 
XRDR =-8. 180E-04*0.5*RHO*L**3 
XW = 5.290E-02*0.5*RHO*L**2 
XWW = 1 . 710E-01*0 .5*RHO*L**2 
XVDR = 1.730E-03*0.5*RHO*L**2 
XWDS = 4 . 600E-02*0 .5*RHO*L**2 
XWDB = 9.660E-03*0.5*RHO*L**2 
XDSDS =-1.160E-02*0.5*RHO*L**2 
XDBDB =-8.070E-03*0.5*RHO*L**2 
XDRDR =-1.010E-02*0.5*RHO*L**2 
XRES = CD0*0.5*RHO*L**2 

XPROP = XRES*ALPHA**2 

SWAY HYDRODYNAMIC COEFFICIENTS 

YPDOT = 1.270E-04*0.5*RHO*L**4 
YRDOT = 1 . 240E-03*0 .5*RHO*L**4 
YPQ = 4.125E-03*0.5*RHO*L**4 
YQR =-6.510E-03*0.5*RHO*L**4 
YVDOT =-5.550E-02*0.5*RHO*L**3 
YP = 3.055E-03*0.5*RHO*L**3 
YR = 2.970E-02*0.5*RHO*L**3 
YVQ = 2 . 360E-02*0 .5*RHO*L**3 
YWP = 2.350E-01*0.5*RHO*L**3 
YWR =-l . 880E-02*0 .5*RHO*L**3 
YV =-9.310E-02*0.5*RHO*L**2 
YVW = 6.840E-02*0.5*RHO*L**2 
YDRS =+2.270E-02*0.5*RHO*L**2 
YDRB =+2 .270E-02*0.5*RHO*L**2 
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HEAVE HYDRODYNAMIC COEFFICIENTS 

ZQDOT =-6.810E-03*0.5*RHO*L**4 
ZPP = 1.270E-04*0.5*RHO*L**4 
ZPR = 6. 670E-03*0.5*RHO*L**4 
ZRR =-7.350E-03*0.5*RHO*L**4 
ZWDOT =-2.430E-01*0.5*RHO*L**3 
ZQ =-l . 350E-01*0 .5*RHO*L**3 
ZVP =-4.810E-02*0.5*RHO*L**3 
ZVR = 4.550E-02*0.5*RHO*L**3 
ZW =-3.020E-01*0.5*RHO*L**2 
ZW =-6.840E-02*0.5*RHO*L**2 
ZDS =-2.270E-02*0.5*RHO*L**2 
ZDB =-2.270E-02*0.5*RHO*L**2 

ROLL HYDRODYNAMIC COEFFICIENTS 

KPDOT =-l .010E-03*0.5*RHO*L**5 
KRDOT =-3 . 370E-05*0.5*RHO*L**5 
KPQ =-6.930E-05*0.5*RHO*L**5 
KQR = 1.680E-02*0.5*RHO*L**5 
KVDOT = 1 .270E-04*0.5*RHO*L**4 
KP =-1.100E-02*0,5*RHO*L**4 
KR =-8.410E-04*0.5*RHO*L**4 
KVQ =-5.115E-03*0.5*RHO*L**4 
KWP =-l .270E-04*0.5*RHO*L**4 
KWR = 1 .390E-02*0.5*RHO*L**4 
KV = 3.055E-03*0.5*RHO*L**3 
KVW =-1.870E-01*0.5*RHO*L**3 

PITCH HYDRODYNAMIC COEFFICIENTS 

MQDOT =-1.680E-02*0.5*RHO*L**5 
MPP = 5.260E-05*0.5*RHO*L**5 
MPR = 5.040E-03*0.5*RHO*L**5 
MRR =-2.860E-03*0.5*RHO*L**5 
MWDOT =-6.810E-02*0.5*RHO*L**4 
MQ =-6.860E-02*0.5*RHO*L**4 
MVP = 1 . 180E-03*0.5*RHO*L**4 
MVR = 1 .730E-02*0.5*RHO*L**4 
MW = 9.860E-02*0.5*RHO*L**3 
MW =-2.510E-02*0,5*RHO*L**3 
MDS =-1.113E-02*0.5*RHO*L**3 
MDB = 1.113E-02*0,5*RHO*L**3 

YAW HYDRODYNAMIC COEFFICIENTS 

NPDOT =-3.370E-05*0.5*RHO*L**5 
NRDOT =-3.400E-03*0.5*RHO*L**5 
NPQ =-2. 110E-02*0.5*RHO*L**5 
NQR = 2.750E-03*0.5*RHO*L**5 
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NVDOT = 1.240E-03*0.5*RHO*L**4 
NP =-8 . 405E-04*0 . 5*RHO*L**4 
NR =-1.640E-02*0.5*RHO*L**4 
NVQ =-9.990E-03*0.5*RHO*L**4 
NWP =-l . 750E-02*0 . 5*RHO*L**4 
NWR = 7.350E-03*0.5*RHO*L**4 
NV =-7.420E-03*0.5*RHO*L**3 
NVW =-2.670E-02*0.5*RHO*L**3 
NDRS =-l . 113E-02*0 . 5*RHO*L**3 
NDRB =+1.113E-02*0.5*RHO*L**3 

OPEN DATA AND RESULTS FILES 

OPEN ( 10, FILE=' PATH_3D.DAT' , STATUS= ' OLD ' ) 
OPEN (11,FILE='XY.RES' ,STATUS='NEW ) 

OPEN ( 12 , FILE= ■ XZ . RES ' , STATUS= ‘ NEW ’ ) 

OPEN (13,FILE=’DRS.RES' ,STATUS=’NEW’ ) 

OPEN (14,FILE= 'DS.RES ' , STATUS= ' NEW ) 

OPEN (15,FILE=’YCTE.RES' , STATUS= ’ NEW ’ ) 
OPEN (16,FILE='ZCTE.RES' , STATUS= ’ NEW ' ) 
OPEN ( 17 , FILE= ' XYZ . RES ' , STATUS= ’ NEW ' ) 

OPEN (18,FILE='U.RES' , STATUS= ' NEW ' ) 

OPEN (19,FILE=’RPM.RES‘ , STATUS= ' NEW ' ) 

OPEN (20,FILE='PHI.RES' , STATUS= ' NEW ' ) 

OPEN ( 2 1,FILE=' THETA. RES' , STATUS= ' NEW ' ) 
OPEN (22,FILE='PSI.RES' , STATUS= ' NEW ' ) 

OPEN (23,FILE='V.RES' , STATUS= ' NEW ' ) 

OPEN (24,FILE='R.RES' , STATUS= ' NEW ' ) 

OPEN (25,FILE= 'W.RES ’ , STATUS= 'NEW ) 

OPEN (26,FILE='Q.RES' , STATUS= ’ NEW ) 

OPEN (27,FILE='YZ.RES' ,STATUS='NEW ) 

READ DATA FILE 

READ (10,*) TSIM, DELTA, IPRNT 
READ (10,*) I PTS, TARGET 
READ (10,*) TN,TH,TV,ZG 
IF (IPTS.GT. 100) IPTS=100 
DO 1 I=1,IPTS 

READ (10,*) XD, YD,ZD,XDH,XDV,U0 
XDES( I )=XD*L 
YDES( I )=YD*L 
ZDES( I)=ZD*L 
UDES( I)=U0 
DISH( I )=XDH*L 
DISV( I)=XDV*L 
1 CONTINUE 

MASS MATRIX INITIALIZATION AND DEFINITION 



DO 15 J=l,6 
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DO 10 K=l,6 

XMMINV(J,K)=0.0 
MM( J,K)=0.0 

10 CONTINUE 
15 CONTINUE 

C 

MM(1,1)= MASS-XUDOT 
MM(1,5)= MASS*ZG 
MM( 1, 6)=-MASS*YG 

MM(2,2)= MASS-YVDOT 
MM( 2 , 4 ) =-MASS*ZG-YPDOT 
MM(2,6)= MASS*XG-YRDOT 

MM(3,3)= MASS-ZWDOT 
MM(3,4)= MASS*YG 
MM( 3 , 5 ) =-MASS*XG-ZQDOT 

MM( 4 , 2 ) =-MASS*ZG-KVDOT 
MM(4,3)= MASS*YG 
MM (4,4)= IX-KPDOT 
MM(4,5)=-IXY 
MM(4, 6)=-IXZ-KRDOT 

MM(5, 1)= MASS*ZG 
MM(5, 3)=-MASS*XG-MWDOT 
MM(5,4)=-IXY 
MM (5,5)= lY-MQDOT 
MM(5, 6)=-IYZ 

MM(6, 1)=-MASS*YG 
MM(6,2)= MASS*XG-NVDOT 
MM ( 6 , 4 ) =- I XZ -NPDOT 
MM(6,5)=-IYZ 
MM(6,6)= IZ-NRDOT 

MASS MATRIX INVERSION 

DO 12 1=1,6 
DO 11 J=l,6 

XMMINV( I, J)=0.0 

11 CONTINUE 
XMMINV(I,I)=1.0 

12 CONTINUE 

CALL INVTA(MM, 6, INDX,D) 

DO 13 J=l,6 

CALL INVTB(MM, 6, INDX,XMMINV( 1, J) ) 

13 CONTINUE 

VARIABLES INITIALIZATION 
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PISIM 


=TSIM/DELTA 


ISIM 


=PISIM 


ECHO 


= 1 . 0/DELTA 


lECHO 


=ECHO 


YAW 


= 0.0 


SWAY 


= 0.0 


PITCH 


= 0.0 


HEAVE 


= 0 . 0 


U 


=UDES( 1 ) 


RPM 


=UDES(1) /ALPHA 


V 


= 0.0 


W 


= 0.0 


P 


= 0 . 0 


Q 


= 0.0 


R 


= 0.0 


DS 


= 0.0 


DB 


= 0 . 0 


DR 


= 0.0 


TWOPI 


=8 . 0*ATAN( 1 . 0 ) 


PI 


=0 .5*TWOPI 


PHI 


= 0 . 0 


I START 


’=1 


TARGET 


'=TARGET*L 


XPOS 


= 0.0 


YPOS 


= 0.0 


ZPOS 


= 0 . 0 


CDY 


= 0.5 


CDZ 


= 0.5 


JPRNT 


= 0 


UK 


= 0 


JE 


= 0 


DRS 


= 0 . 0 


DRB 


= 0 . 0 


DS 


= 0.0 


DB 


= 0.0 


DEFINE 


; THE LENGTH X, 


X(l) 


= -105.9/12.0 


X(2) 


= -99.3/12.0 


X(3) 


= -87.3/12.0 


X(4) 


= -66.3/12.0 


X(5) 


= 72.7/12.0 


X(6) 


= 83.2/12.0 


X(7) 


= 91.2/12.0 


X(8) 


= 99.2/12.0 


X(9) 


= 103.2/12.0 



BREADTH BR, 



AND HEIGHT HH TERMS 



HH(1) = 0.00/12.0 
HH(2) = 8.24/12.0 
HH(3) = 19.76/12.0 
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HH(4) 


= 


29.36/12.0 


HH(5) 


= 


31.85/12.0 


HH(6) 


= 


27.84/12.0 


HH(7) 


= 


21.44/12.0 


HH(8) 


= 


12.00/12.0 


HH(9) 


= 


0.00/12.0 


BR(1) 


= 


0.00/12.0 


BR(2 ) 


= 


8.24/12.0 


BR(3) 


= 


19.76/12.0 


BR(4) 


= 


29.36/12.0 


BR(5) 


= 


31.85/12.0 


BR( 6) 


= 


27.84/12.0 


BR(7) 


= 


21.44/12.0 


BR(8) 


= 


12.00/12.0 


BR(9) 


= 


0.00/12.0 



AUXILLIARY VARIABLES FOR HORIZONTAL PLANE CONTROL 

DH =(IZ-NRDOT)*(MASS-YVDOT)- 
& (MASS*XG-YRDOT) *(MASS*XG-NVDOT) 

A11H=( ( IZ-NRDOT) *YV-(MASS*XG-YRDOT) *NV)/DH 
A12H=( ( IZ-NRDOT) *(-MASS+YR)- 
& (MASS*XG-YRDOT) *(-MASS*XG+NR) ) /DH 

A21H=( (MASS-YVDOT)*NV-(MASS*XG-NVDOT) *YV)/DH 
A22H= ( (MASS-YVDOT) * ( -MASS*XG+NR) - 
& (MASS*XG-NVDOT)*(-MASS+YR) )/DH 

B1 1H= ( ( IZ-NRDOT) *YDRS- (MASS*XG-YRDOT) *NDRS ) /DH 
B12H= ( ( IZ-NRDOT) *YDRB- (MASS*XG-YRDOT ) *NDRB ) /DH 
B21H= ( (MASS-YVDOT ) *NDRS- (MASS*XG-NVDOT ) * YDRS ) /DH 
B22H=( (MASS-YVDOT )*NDRB-(MASS*XG-NVDOT)*YDRB)/DH 
BIH =B11H-B12H 
B2H =B21H-B22H 

AUXILLIARY VARIABLES FOR VERTICAL PLANE CONTROL 

DV =(MASS-ZWDOT)* ( lY-MQDOT ) -ZQDOT*IWDOT 
A11V=( ( lY-MQDOT) *ZW+ZQDOT*MW)/DV 
A12V= ( ( I Y-MQDOT ) * ( ZQ+MASS ) +ZQDOT*MQ ) /DV 
A13V=-(ZG-ZB)* (MASS*XG+ZQDOT)*WEIGHT/DV 
A21V=(MWDOT*ZW+(MASS-ZWDOT) *MW)/DV 
A22V= (MWDOT*( ZQ+MASS )+ (MASS-ZWDOT) *MQ)/DV 
A23V=-(ZG-ZB)* (MASS-ZWDOT)*WEIGHT/DV 
B11V=( ( IY-MQDOT)*ZDS+ZQDOT*MDS)/DV 
B12V= ( ( lY-MQDOT) *ZDB+ZQDOT*MDB ) /DV 
B2 lV=(MWDOT*ZDS+( MASS-ZWDOT ) *MDS) /DV 
B 2 2 V= ( MWDOT *ZDB+(MASS- Z WDOT ) *MDB ) / DV 
BIV =B11V-B12V 
B2V =B21V-B22V 
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SIMULATION BEGINS 



LOOP OVER WAY POINTS 

DO 200 IP=1, IPTS 

IF (IP.GE.2) GO TO 210 
XDH=DISH(1) 

XDV=DISV( 1 ) 

UO =UDES(1) 

XD =XDES(1) 

YD =YDES(1) 

ZD =ZDES(1) 

XD1=0.0 

YD1=0.0 

ZD1=0.0 

XD2=XD 

YD2=YD 

ZD2=ZD 

GO TO 211 

210 XDH=DISH(IP) 

XDV=DISV( IP) 

UO =UDES(IP) 

XD =XDES(IP) 

YD =YDES(IP) 

ZD =ZDES(IP) 

XD1=XD2 

YD1=YD2 

ZD1=ZD2 

XD2=XD 

YD2=YD 

ZD2=ZD 

211 ZD12=ZD2-ZD1 
XD12=XD2-XD1 
YD12=YD2-YD1 

HORIZONTAL HEADING CONTROL GAINS 

OMEGAH=(10.0*U0)/(TH*L) 

AD1H=1 . 75*OMEGAH 

AD2H=2 . 15*OMEGAH**2 

AD3H=OMEGAH**3 

A1=B1H*U0*U0 

B1=B2H*U0*U0 

C1=-AD1H-(A11H+A22H)*U0 

A2=(B1H*A22H-B2H*A12H) *U0**3 

B2=(B2H*A11H-B1H*A21H)*U0**3 

KlH=AD3H/( (B2H*A11H-B1H*A21H) *U0**3) 

C2=AD2H-(A11H*A22H-A12H*A21H)*U0**2+B2H*U0*U0*K1H 

K2H=(C1*B2-C2*B1 )/(Al*B2-A2*Bl ) 

K3H=(C2*A1-C1*A2)/(A1*B2-A2*B1 ) 
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VERTICAL HEADING CONTROL GAINS 

OMEGAV= (10. 0 *U0 ) / ( TV*L ) 

AD1V=1 . 75*OMEGAV 

AD2V=2 . 15*OMEGAV**2 

AD3V=OMEGAV**3 

A2=B1V*U0*U0 

A3=B2V*U0*U0 

D1=-AD1V- (A11V+A22V) *U0 

B1=-B2V*U0*U0 

B2=(B1V*A22V-B2V*A12V) *U0**3 

B3=(B2V*A11V-B1V*A21V)*U0**3 

D2=AD2V+A23V+(A12V*A21V-A11V*A22V)*U0**2 

C1=(B2V*A11V-B1V*A21V)*U0**3 

C2=(A23V*B1V-A13V*B2V) *U0**2 

D3=AD3V+ ( A1 3V*A2 IV-Al 1V*A2 3V ) *U0 

K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 

K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*B2 ) 

K1V=(D3-C2*K2V)/C1 

K3V=(D1-A2*K2V)/A3 

ALPHAH=ATAN(YD12/XD12) 

ALPHAH=ABS ( ALPHAH ) 

IF ( (XD12.GE.0.0) .AND. (YD12.GE.0.0) ) ALPHAH= ALPHAH 
IF ( (XD12.GE. 0.0) .AND. (YD12.lt. 0.0) ) ALPHAH= -ALPHAH 
IF ( (XD12.lt. 0.0) .AND. ( YD12 . GE . 0 . 0 ) ) ALPHAH=PI -ALPHAH 
IF ( (XD12.lt. 0.0) .AND. ( YD12 . LT . 0 . 0 ) ) ALPHAH=PI+ALPHAH 
XCTEH=(YPOS-YDl) *SIN(ALPHAH)+(XPOS-XDl ) *COS (ALPHAH) 
YCTE = ( YPOS - YDl ) *COS ( ALPHAH ) - ( XPOS -XDl ) * S IN ( ALPHAH ) 
XIP =YD12 * SIN ( ALPHAH )+XD12*COS( ALPHAH) 

ALPH AV=ATAN (ZD12/X1P) 

ALPHAV=ABS ( ALPHAV ) 

IF (ZD12 .GE.0.0) ALPH AV= -ALPHAV 

K4V=- (A13V* (A21V+B2V*U0*K2V) -A23V* ( A11V+B1V*U0*K2V) ) 
K4V=K4V*SIN(ALPHAV)/( (B1V*A21V-B2V*A11V) *U0*U0 ) 

ZCTE = (ZPOS-ZDl)*COS(ALPHAV)+XCTEH*SIN(ALPHAV) 
XCTEV=-( ZPOS-ZDl) *SIN( ALPHAV) +XCTEH*COS( ALPHAV) 

PROPULSION CONTROL GAIN 

WSS=(B1V*A23V-B2V*A13V)*SIN( ALPHAV) 

WSS=WSS/ ( ( A11V*B2V-A21V*B1V) *U0 ) 
DSS=(A21V*A13V-A11V*A23V)*S IN (ALPHAV) 

DSS=DSS/( (A11V*B2V-A21V*B1V) *U0*U0) 
FUC=XWW*WSS**2+U0*WSS*(XWDS-XWDB)*DSS 
& +U0*U0*(XDSDS+XDBDB)*DSS**2-XRES*U0**2 

RPM0=-FUC/ ( XRES *ALPHA* * 2 ) 

RPM0=SQRT(RPM0) 

WRITE (*,*) RPM0,U0 /ALPHA 

KN=-5 . 0*U0* (MASS-XUDOT) / (XRES*ALPHA*ALPHA*RPMO*TN*L) 
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WRITE (*,201) XD/L, YD/L, ZD/L 

SIMULATION FOR EACH WAY POINT 

DO 100 I=ISTART, ISIM 
ICOUNT=I 

IF (U.LT.UMIN) U=UMIN 

CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER 

THE VEHICLE 



DO 600 K=l,9 

UCF=(V+X(K)*R)**2+(W-X(K)*Q)**2 

UCF=SQRT(UCF) 

IF (UCF.LT. 1 .E-6) GO TO 601 
CFLOW=CDY*HH(K) * (V+X(K) *R) **2+CDZ*BR(K)* 
& (W-X(K)*Q)**2 

VECHl ( K ) =CFLOW* ( V+X ( K ) *R ) /UCF 
VECH2 ( K ) =CFLOW* ( V+X ( K ) *R ) *X ( K ) /UCF 
VECVl ( K ) =CFLOW* ( W-X ( K ) *Q ) /UCF 
VECV2 ( K ) =CFLOW* ( W-X ( K ) *Q ) *X ( K ) /UCF 

600 CONTINUE 

CALL TRAP (9, VECVl, X, HEAVE) 

CALL TRAP(9,VECV2,X,PITCH) 

CALL TRAP(9,VECH1,X, SWAY ) 

CALL TRAP ( 9 , VECH2 , X , YAW ) 

HEAVE=-0 . 5*RHO*HEAVE 
PITCH=+0 . 5*RHO*PITCH 
SWAY =-0 . 5*RHO*SWAY 
YAW =-0.5*RHO*YAW 
GO TO 602 

601 HEAVE=0.0 
PITCH=0.0 
SWAY =0.0 
YAW =0.0 

602 CONTINUE 

FORCE EQUATIONS 



SURGE FORCE 

FP(1) = MASS*V*R-MASS*W*Q+MASS*XG*Q**2+MASS*XG*R**2- 
& MASS*YG*P*Q-MASS*ZG*P*R+XPP*P**2+XQQ* 

& Q**2+XRR*R**2+XPR*P*R+XWQ*W*Q+XVP*V*P+ 

& XVR*V*R+U*Q* (XQDS*DS+XQDB*DB)+ 

& U*R* (XRDRS*DRS+XRDRB*DRB)+XW*V**2+XWW* 

& W**2+U*V*(XVDRS*DRS+XDRB*DRB)+U*W* 

& (XWDS*DS+XWDB*DB)+(XDSDS*DS**2+XDBDB*DB**2+ 

& XDRDR* (DRS**2+DRB**2 ) ) *U**2- (WEIGHT-BOY) * 
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& SIN( THETA) +XPROP*RPM*RPM-XRES*U*U 

SWAY FORCE 



FP(2 ) =-MASS*U*R-MASS*XG*P*Q+MASS*YG*R**2-MASS*ZG*Q*R+ 
& YPQ*P*Q+YQR*Q*R+YP*U*P+YR*U*R+YVQ*V*Q+ 

& YWP*W*P+YWR*W*R+YV*U*V+YVW*V*W+YDRS*U**2*DRS+ 

& YDRB*U**2*DRB+(WEIGHT-B0Y)* 

& COS(THETA) *SIN(PHI )+MASS*W*P+MASS*YG*P**2+SWAY 



HEAVE FORCE 



& 

& 

& 

& 

& 



FP(3) = MASS*U*Q-MASS*V*P-MASS*XG*P*R-MASS*YG*Q*R+ 
MASS*ZG*P**2+MASS*ZG*Q**2+ZPP*P**2+ 
ZPR*P*R+ZRR*R**2+ZQ* 

U*Q+ZVP*V*P+ZVR*V*R+ZW*U*W+Z\T'/*V**2+HEAVE+ 
U**2* ( ZDS*DS+ZDB*DB)+ (WEIGHT-BOY) * 

COS ( THETA ) * COS ( PHI ) 



ROLL MOMENT 



& 

& 

& 

& 

& 

& 



& 

& 

& 

& 

Sc 

Sc 

Sc 

Sc 



Sc 

Sc 

Sc 

Sc 

Sc 

Sc 



FP(4) = -IZ*Q*R+IY*Q*R-IXY*P*R+IYZ*Q**2- 

IYZ*R**2+IXZ*P*Q+MASS*YG*U*Q-MASS* 

YG*V*P-MASS*ZG*W*P+KPQ*P*Q+KQR*Q*R+ 

KP*U*P+KR*U*R+KVQ*V*Q+KWP*W*P+ 

KWR*W*R+KV*U*V+KVW*V*W+ ( YG*WE IGHT- YB*BOY ) * 

COS ( THETA) *COS ( PHI ) - ( ZG*WEIGHT- 

ZB*BOY) *COS( THETA) * SIN (PHI) +MASS*ZG*U*R 

PITCH MOMENT 

FP(5) = -IX*P*R+IZ*P*R+IXY*Q*R-IYZ*P*Q- 
IXZ*P**2+IXZ*R**2-MASS*XG*U*Q+ 
MASS*XG*V*P+MASS*ZG*V*R- 
MASS*ZG*W*Q+MPP*P**2+ 

MPR*P*R+MRR*R**2+MQ* 
U*Q+MVP*V*P+MVR*V*R+MW*U*W+ 
MW*V**2+U**2*(MDS*DS+MDB*DB)-(XG*WEIGHT- 
XB*BOY) *COS( THETA) *COS( PHI )- 
( ZG*WEIGHT-ZB*BOY ) * SIN ( THETA ) +P ITCH 

YAW MOMENT 

FP(6) = -IY*P*Q+IX*P*Q+IXY*P**2-IXY*Q**2+IYZ*P*R- 
IXZ*Q*R-MASS*XG*U*R+MASS*XG*W*P-MASS*YG* 
V*R+MASS*YG*W*Q+NPQ*P*Q+NQR*Q*R+NP*U*P+NR* 
U*R+NVQ*V*Q+NWP*W*P+NWR*W*R+NV*U*V+ 

NVW* V*W+NDRS*U* * 2 *DRS+NDRB*U* * 2*DRB+ 
(XG*WEIGHT-XB*BOY)*COS(THETA)*SIN(PHI)+ 

( YG*WEIGHT-YB*BOY) * SIN (THETA) +YAW 
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COMPUTE THE RIGHT HAND SIDE OF XDOT=F(X) 

DO610J=I,6 
F( J) = 0.0 
DO 611 K = 1,6 

F(J) = XMMINV( J,K)*FP(K) + F(J) 

611 CONTINUE 

610 CONTINUE 

INERTIAL POSITION RATES 



& 

& 



S, 

& 



& 



F(7) = U*COS(PSI)*COS(THETA)+V*(COS(PSI)*SIN(THETA)* 
SIN(PHI )-SIN(PSI ) *COS(PHI ) )+W* (COS(PSI ) * 
SIN(THETA)*COS(PHI)+SIN(PSI)*SIN(PHI) ) 

F(8) = U*SIN(PSI)*COS(THETA)+V*(SIN(PSI)*SIN(THETA)* 
SIN(PHI )+COS(PSI )*COS(PHI) )+W* (SIN(PSI ) * 

SIN ( THETA ) *COS ( PHI ) -COS (PSI)* SIN (PHI) ) 

F(9) = -U*SIN(THETA)+V*COS(THETA)*SIN(PHI)+ 

W*COS ( THETA )*COS( PHI) 



EULER ANGLE RATES 



F ( 1 0 ) = P+Q* S IN ( PH I ) *TAN ( THETA ) +R*COS ( PHI ) 
F(ll)= Q*COS(PHI )-R*SIN(PHI ) 

F( 12 )= Q*SIN(PHI ) /COS (THETA )+R*COS( PHI ) 
ASSIGN XDOT VECTOR 



*TAN( THETA) 



/COS (THETA) 



UDOT = F ( 
VDOT = F ( 
WDOT = F ( 
PDOT = F ( 
QDOT = F ( 
RDOT = F( 
XDOT = F ( 
YDOT = F ( 
ZDOT = F( 
PHIDOT = F( 
THEDOT = F( 
PSIDOT = F( 

FIRST ORDER 



1 ) 

2 ) 

3) 

4) 

5) 

6 ) 

7) 

8 ) 

9) 

10) 

11 ) 

12) 

INTEGRATION 



U 

V 

W 



U + DELTA*UDOT 
V + DELTA*VDOT 
W + DELTA*WDOT 
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P 

Q 

R 

XPOS 

YPOS 

ZPOS 

PHI 

THETA 

PSI 



P + DELTA*PDOT 

Q + DELTA*QDOT 

R + DELTA*RDOT 

XPOS + DELTA*XDOT 
YPOS + DELTA*YDOT 
ZPOS + DELTA* ZDOT 
PHI + DELTA*PHIDOT 
THETA + DELTA*THEDOT 
PSI + DELTA*PSID0T 



VELOCITY INPUT CALCULATION 



UC=U0 

IF (UC.GE.UMAX) UC=UMAX 
IF (UC.LE.UMIN) UC=UMIN 

RPM INPUT CALCULATION 

RPMO=UC/ALPHA 
RPM=RPMO+KN* ( U-UC ) 

IF ( RPM . GE . RPMMAX ) RPM=RPMMAX 
IF (RPM.LE.RPMMIN) RPM=RPMMIN 

COORDINATE TRANSFORMATIONS 

XCTEH= ( YPOS-YDl ) *SIN ( ALPHAH ) + ( XPOS-XDl ) *COS ( ALPHAH ) 
YCTE = (YPOS-YDl)*COS(ALPHAH)-(XPOS-XDl)*SIN(ALPHAH) 
ZCTE = (ZPOS-ZDl)*COS(ALPHAV)+XCTEH*SIN(ALPHAV) 
XCTEV=- ( ZPOS-ZDl ) *SIN( ALPHAV) +XCTEH*COS ( ALPHAV) 

HIT CRITERIA 

VT0TAL=(XD2-XD1)**2+(ZD2-ZD1)**2 
VTOTAL= SQRT ( VTOTAL ) 

HTOTAL=(XD2-XDl )**2+(YD2-YDl )**2 
HTOTAL= SQRT ( HTOTAL ) 

VAWAY =VTOTAL-XCTEV 
VAWAY =ABS( VAWAY) 

HAWAY =HTOTAL-XCTEH 
HAWAY =ABS( HAWAY) 

IF (( VAWAY.lt. TARGET) .OR. ( HAWAY.lt. TARGET ) ) GO TO 
& 101 

DIVE PLANE INPUT CALCULATION 
ZPHI=ZCTE 

S IGV=ATAN ( ZPH I /XDV ) 

DS=K1V* ( THETA-ALPHAV-S IGV ) +K2V*W+K3V*Q+K4V 

IF (DS.GE. 0.4) DS= 0.4 
IF (DS.LE.-0.4) DS=-0.4 



85 



ooo o o ooo 



c 



DB=-DS 



RUDDER INPUT CALCULATION 
YPHI=YCTE 

S IGH= - AT AN ( YPH I /XDH ) 

DRS=K1H* (PSI-ALPHAH-SIGH)+K2H*V+K3H*R 

IF (DRS.GE. 0.4) DRS= 0.4 
IF (DRS.LE.-0.4) DRS=-0.4 

DRB=-DRS 

PRINT RESULTS 

TIME=I*DELTA 
JE=JE+1 

IF ( JE.NE. lECHO) GO TO 99 
JE=0 

99 JPRNT=JPRNT+1 

IF ( JPRNT.NE. IPRNT) GO TO 100 
IJK=IJK+1 
TIME=I*DELTA 

WRITE (11,*) XPOS/L, YPOS/L 
WRITE (12,*) XPOS/L, ZPOS/L 
WRITE (13,*) TIME, DRS*180. 0/PI 
WRITE (14,*) TIME, DS*180. 0/PI 
WRITE (15,*) TIME,YCTE/L 
WRITE (16,*) TIME,ZCTE/L 
WRITE (17,*) XPOS/L, YPOS/L, ZPOS/L 
WRITE (18,*) TIME,U 
WRITE (19,*) TIME,RPM 
WRITE (20,*) TIME, PHI*I80. 0/PI 
WRITE (21,*) TIME, (THETA-ALPHAV)*180. 0/PI 
WRITE (22,*) TIME, (PSI-ALPHAH)*180. 0/PI 
WRITE (23,*) TIME,V 
WRITE (24,*) TIME,R 
WRITE (25,*) TIME,W 
WRITE (26,*) TIME,Q 
WRITE (27,*) YPOS/L, ZPOS/L 
JPRNT=0 
C 

100 CONTINUE 
GO TO 500 

101 ISTART=ICOUNT 

200 CONTINUE 
500 STOP 

201 FORMAT (’ HEADING FOR (X,Y,Z) = ( ',F9.3,' , ',F9.3,' 

& ' ,F9 . 3 ' ) ' ) 

END 
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SUBROUTINE TRAP ( N , A, B , OUT ) 

NUMERICAL INTEGRATION ROUTINE USING THE TRAPEZOIDAL RULE 

DIMENSION A(1),B(I) 

N1=N-1 
OUT=0.0 
DO 1 1=1, N1 

OUT1=0.5*(A(I)+A(I+1) )*(B(I+1)-B(I)) 

OUT =OUT+OUTl 
1 CONTINUE 
RETURN 
END 



SUBROUTINE INVTA(MM,N, INDX, D) 

PARAMETER (NMAX=100 , TINY=1 . OE-20 ) 

DIMENSION INDX( 6) , W(NMAX) 

REAL MM(6,6) 

D=1 

DO 12 1=1, N 
AAMAX=0. 

DO 11 J=1,N 

IF(ABS(MM( I, J) ) .GT.AAMAX) AAMAX=ABS ( MM( I , J ) ) 

11 CONTINUE 

IF (AAMAX.EQ. 0 . ) PAUSE ’ SINGULAR MATRIX ’ 

W(I) = 1./AAMAX 

12 CONTINUE 

DO 19 J=1,N 
DO 14 1=1, J-1 
SUM=MM(I, J) 

DO 13 K=l, I-l 

SUM=SUM-MM( I,K) *MM(K, J) 

13 CONTINUE 
MM( I, J)=SUM 

14 CONTINUE 
AAMAX=0 . 

DO 16 I=J,N 
SUM=MM(I, J) 

DO 15 K=1,J-1 

SUM=SUM-MM(I,K)*MM(K, J) 

15 CONTINUE 
MM(I, J)=SUM 
DUM=W(I)*ABS(SUM) 

IF ( DUM . GE . AAMAX ) THEN 
IMAX=I 
AAMAX=DUM 
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ENDIF 

16 CONTINUE 

IF ( J.NE. IMAX)THEN 
DO 17 K=1,N 

DUM=MM( IMAX,K) 

MM( IMAX,K)=MM( J,K) 

MM( J,K)=DUM 

17 CONTINUE 
D=-D 

W( IMAX)=W( J) 

ENDIF 

INDX( J)=IMAX 

IF(MM( J, J) .EQ.O. )MM( J, J)=TINY 
IF( J.NE. N) THEN 
DUM=1 . /MM( J, J) 

DO 18 I=J+1,N 

MM( I, J)=MM( I, J)*DUM 



18 


CONTINUE 




ENDIF 


19 


CONTINUE 




RETURN 




END 



SUBROUTINE INVTB ( MM, N , INDX, B ) 
DIMENSION INDX(N),B(N) 

REAL MM(6,6) 

11 = 0 . 

DO 12 1=1, N 
LL=INDX( I) 

SUM=B(LL) 

B(LL)=B( I) 

IF (II.NE.O)THEN 
DO 11 J=II,I-1 

SUM=SUM-MM ( I , J ) *B ( J ) 

11 CONTINUE 

ELSE IF (SUM.NE.O) THEN 
II = I 
ENDIF 
B( I)=SUM 

12 CONTINUE 

DO 14 I=N,1,-1 
SUM=B( I ) 

IF (I.LT.N)THEN 
DO 13 J=I+1,N 

SUM=SUM-MM( I , J) *B( J) 

13 CONTINUE 
ENDIF 

B( I)=SUM/MM( I, I) 

14 CONTINUE 
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RETURN 

END 
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C 

C 

c 

c 

c 

c 

c 

c 



c 



c 



c 



PROGRAM VERT_STAB.FOR 

REGIONS OF STABILITY - VERTICAL PLANE 
PARAMETERS ARE: XD AND TV 
NUMERICAL OR ANALYTIC COMPUTATION 

IT NEEDS FILE " SUBRTNS . FOR" OR ANY STANDARD EIGENVALUE 

SOLVER 



IMPLICIT DOUBLE PRECISION (A-H,0-Z) 

DOUBLE PRECISION KIV, K2V, K3V, L 

DOUBLE PRECISION MQDOT, MQ , MW, MWDOT, MDS , MDB , MASS , lY 
DIMENS ION A(4,4),FV1(4), IVI (4),ZZZ(4,4),WR(4),WI(4) 

OPEN ( I0,FILE=' BIFO.RES ’ , STATUS= ' NEW' ) 

OPEN (I1,FILE='BIF1.RES' , STATUS= ' NEW ' ) 

OPEN (I2,FILE=’BIF2.RES' , STATUS= ' NEW ’ ) 

OPEN (13,FILE='BIF3.RES' , STATUS= ' NEW ' ) 



WEIGHT=12000.0 
lY = 9450.0 

L = 17.425 

RHO = 1.94 

G = 32.2 

XG = 0.0 

ZB = 0.0 

MASS =WEIGHT/G 
BOY =WEIGHT 

ZQDOT =-6.810E-03*0.5*RHO*L**4 
ZWDOT =-2.430E-01*0.5*RHO*L**3 
ZQ =-l . 350E-01*0 .5*RHO*L**3 
ZW =-3.020E-01*0.5*RHO*L**2 
ZDS =-2.270E-02*0.5*RHO*L**2 
ZDB =-2.270E-02*0.5*RHO*L**2 
MQDOT =-l . 680E-02*0 . 5*RHO*L**5 
MWDOT =-6.810E-02*0.5*RHO*L**4 
MQ =-6.860E-02*0.5*RHO*L**4 
MW = 9 . 860E-02*0 . 5*RHO*L**3 
MDS =-l. 113E-02*0.5*RHO*L**3 
MDB = 1 . 113E-02*0.5*RHO*L**3 

WRITE (*,1001) 

READ (*,*) TVMIN,TVMAX, ITV 
WRITE (*,1002) 

READ (*,*) XDMIN,XDMAX, IXD 
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XDMIN=XDMIN*L 
XDMAX=XDMAX*L 
WRITE (*,1003) 

READ (*,*) U,ZG 

WRITE (*,1004) 

READ (*,*) ISOL 

AUXILIARY VARIABLES 

DV =(MASS-ZWDOT)*( IY-MQDOT)-ZQDOT*MWDOT 
A1 1 V= ( ( I Y-MQDOT ) * ZW+ZQDOT*MW ) /DV 
AI2V= ( ( I Y-MQDOT) * ( ZQ+MASS ) +ZQDOT*MQ ) /DV 
A13V=- ( ZG-ZB ) * (MASS*XG+ZQDOT) *WEIGHT/DV 
A21V=(MWDOT*ZW+(MASS-ZWDOT)*MW)/DV 
A22V=(MWDOT* ( ZQ+MASS )+(MASS-ZWDOT) *MQ) /DV 
A23V=- ( ZG-ZB ) * (MASS-ZWDOT) *WEIGHT/DV 
B11V=( ( IY-MQDOT)*ZDS+ZQDOT*MDS)/DV 
B12V=( ( IY-MQDOT)*ZDB+ZQDOT*MDB)/DV 
B21V=(MWDOT*ZDS+ (MASS-ZWDOT )*MDS)/DV 
B22V=(MWDOT*ZDB+ (MASS-ZWDOT) *MDB)/DV 
BIV =B11V-B12V 
B2V =B21V-B22V 

EPS =l.D-5 
ILMAX=1500 

LOOP OVER TV 

DO 1 1=1, ITV 

WRITE (*,2001) I, ITV 

TV=TVMIN+( I-l)*(TVMAX-TVMIN)/( ITV-1) 
OMEGAV=( 10 .0*U)/(TV*L) 

AD1V=1 . 75*OMEGAV 
AD2V=2 . 15*OMEGAV**2 
AD3V=OMEGAV**3 
A2=B1V*U*U 
A3=B2V*U*U 

D1=-AD1V-(A11V+A22V)*U 

B1=-B2V*U*U 

B2=(B1V*A22V-B2V*A12V)*U**3 

B3=(B2V*A11V-B1V*A21V)*U**3 

D2=AD2V+A23V+(A12V*A21V-A11V*A22V)*U**2 

C1=(B2V*A11V-B1V*A21V) *U**3 

C2=(A23V*B1V-A13V*B2V)*U**2 

D3=AD3V+(A13V*A21V-A11V*A23V)*U 

K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3 ) 

K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*B2) 

K1V=(D3-C2*K2V)/C1 

K3V= ( D1-A2 *K2V ) /A3 

D333=(A13V*A21V-A11V*A23V)*U 

XAAA=CBRT(-D333) 
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D334=(D3*B2*C1*A3+B3*C1*A2*A3-B3*C1*D1*C2-D1*C1*C2*A3) ‘ 

D335=B3*C1*A2+B2*C1*A3-B1*C2*A3 

D336=D334/D335 

XBBB=CBRT(D336) 

ANALYTICAL COMPUTATION 

IF (ZG.NE.0.0) TVCR1=(I0.*U)/(XAAA*L) 

IF (ZG.NE.0.0) TVCR2=(I0.*U)/(XAAA*L) 

IF (ISOL.EQ.O) GO TO 22 
CXD2=AD3V*(AD1V*AD2V-AD3V) 

CXDI=-(B2+A3*U) *K1V* (AD1V*AD2V-AD3V)+AD3V*K1V* 

& (A2*AD1V+B2+A3*U)-AD1V*AD1V*K1V*(-C2+C1*U) 

CXD0=- ( B2+A3*U) *K1V*K1V* ( AD1V*A2+B2+A3*U ) 
DET=CXDl*CXDl-4 . 0*CXD2*CXD0 
IF (DET.LT.0.0) GO TO 1 
XDI=(-CXDI+DSQRT(DET) )/(2.0*CXD2) 

XD2= ( -CXD1-DSQRT( DET ) ) / ( 2 . 0*CXD2 ) 

IF (XDI.NE.0.0) 

& VALI=AD3V+( (B2V*AI2V-BIV*A22V-B2V)*KIV*U**3)/XD1 

IF (XD2.NE.0.0) 

& VAL2=AD3V+( ( B2V*A12V-BIV*A22V-B2V) *K1V*U**3 ) /XD2 

GO TO 23 

NUMERICAL COMPUTATION 
LOOP OVER XD 

22 DO 2 J=I,IXD 

XD=XDMIN+ ( J- 1 ) * ( XDMAX-XDMIN ) / ( IXD- 1 ) 

THETA=0 . ODO 
CT=DCOS( THETA) 

ST=DS IN (THETA) 

W=O.ODO 

A(1,1)=0.0D0 

A(1,2)=0.0D0 

A(I,3)=I.0D0 

A(1,4)=0.0D0 

A(2, I)=B1V*U*U*K1V+A13V*CT 
A(2,2)=BIV*U*U*K2V+A11V*U 
A( 2 , 3 )=B1V*U*U*K3V+A12V*U 
A(2,4)=-B1V*U*U*KIV/XD 
A( 3 , 1 ) =B2V*U*U*K1V+A23V*CT 
A( 3, 2 )=B2V*U*U*K2V+A2IV*U 
A( 3 , 3 )=B2V*U*U*K3V+A22V*U 
A( 3 , 4 ) =-B2V*U*U*KlV/XD 
A(4, I)=-U*CT-W*ST 
A(4,2)=CT 
A(4,3)=0.0D0 
A(4,4)=0.0D0 

COMPUTE EIGENVALUES 
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CALL RG(4,4,A,WR,WI,0,ZZZ, IVl , FVl , lERR) 
CALL DSTABL( DECS, WR,WI, FREQ) 



IF ( J.GT. 1) GO TO 10 

DEOSOO=DEOS 

XDOO =XD 

LL=0 

GO TO 2 

10 DEOSNN=DEOS 

XDNN =XD 
PR=DEOSNN*DEOSOO 
IF (PR.GT.O.DO) GO TO 3 
LL=LL+1 

IF (LL.GT.3) STOP 1000 
IL=0 

XDO=XDOO 
XDN=XDNN 
DEOSO=DEOSOO 
DEOSN=DEOSNN 
6 XDL=XDO 

XDR=XDN 
DEOSL=DEOSO 
DEOSR=DEOSN 
XD=(XDL+XDR)/2 .DO 
A(l, 1)=0.0D0 
A( 1 , 2 )=0 . ODO 
A( 1, 3 )=1 . ODO 
A(1,4)=0.0D0 

A(2, 1 )=B1V*U*U*K1V+A13V*CT 
A(2,2 )=B1V*U*U*K2V+A11V*U 
A(2, 3)=B1V*U*U*K3V+A12V*U 
A(2,4 )=-BlV*U*U*KlV/XD 
A(3, 1 )=B2V*U*U*K1V+A23V*CT 
A(3,2 )=B2V*U*U*K2V+A21V*U 
A(3, 3)=B2V*U*U*K3V+A22V*U 
A( 3 , 4 ) =-B2V*U*U*KlV/XD 
A(4, 1 )=-U*CT-W*ST 
A(4,2)=CT 
A(4,3)=0.0D0 
A(4,4)=0.0D0 

CALL RG(4,4,A,WR,WI,0,ZZZ, IV1,FV1, lERR) 
CALL DSTABL(DEOS,WR,WI,FREQ) 

DEOSM=DEOS 

XDM=XD 

PRL=DEOSL*DEOSM 
PRR=DEOSR*DEOSM 
IF (PRL.GT.O.DO) GO TO 5 
XDO=XDL 
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XDN=XDM 

DEOSO=DEOSL 

DEOSN=DEOSM 

IL=IL+1 



IF ( IL.GT. ILMAX) STOP 3100 
DIF=DABS ( XDL-XDM ) 

IF (DIF.GT.EPS) GO TO 6 

XD=XDM 

GO TO 4 

5 IF (PRR.GT.O.DO) STOP 3200 

XDO=XDM 
XDN=XDR 
DEOSO=DEOSM 
DEOSN=DEOSR 
IL=IL+1 

IF (IL.GT. ILMAX) STOP 3100 
DIF=DABS(XDM-XDR) 

IF (DIF.GT.EPS) GO TO 6 
XD=XDM 

4 LLL=10+LL 

WRITE (LLL,*) XD/L,TV 
3 XDOO=XDNN 

DEOSOO=DEOSNN 
2 CONTINUE 
GO TO 1 

23 IF (VALI.GT.0.0) WRITE (11,*) XD1/L,TV 
IF (VAL2 .GT. 0 . 0) WRITE (12,*) XD2/L,TV 
1 CONTINUE 

IF (ZG.NE.0.0) WRITE (10,*) XDMIN/L , TVCRl 
IF (ZG.NE.0.0) WRITE (10,*) XDMAX/L, TVCRl 



1001 FORMAT 

1002 FORMAT 

1003 FORMAT 

1004 FORMAT 
& 

2001 FORMAT 
END 
C 



(' ENTER MIN, MAX, AND INCREMENTS OF TV) 
(' ENTER MIN, MAX, AND INCREMENTS OF XD ' ) 
( ' ENTER U AND ZG' ) 

(' ENTER 0 : NUMERICAL’,/, 

' 1 : ANALYTICAL') 

(215) 



SUBROUTINE DSTABL ( DEOS , WR , WI , OMEGA ) 
IMPLICIT DOUBLE PRECISION (A-H,0-Z) 
DIMENSION WR(4),WI(4) 

DEOS=-l . OD+20 
DO 1 1=1,4 

IF (WR(I) .LT.DEOS) GO TO 1 
DEOS=WR(I) 

U=I 

1 CONTINUE 
OMEGA=WI( IJ) 

OMEGA=DAB S ( OME G A ) 

RETURN 
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END 

C 

FUNCTION CBRT(A) 

IF (A.GT.0.0) CBRT= A **(l./3.) 
IF (A.LE.0.0) CBRT=-(-A)**(l./3. ) 
RETURN 
END 
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APPENDIX C 



PROGRAM VERT_STEADY . FOR 

COMPUTATION OF STEADY STATE SOLUTIONS IN THE VERTICAL 

PLANE 

(CHAPTER III, PARAGRAPH F) 

REAL K1V,K2V,K3V,L,MQD0T,MQ,MW,MWD0T,MDS,MDB,MASS, lY 
C 

WEIGHT=12000 . 0 
lY = 9450.0 

L = 17.425 

RHO = 1.94 

G = 32.2 

XG = 0.0 

ZB = 0.0 

MASS =WEIGHT/G 
BOY =WEIGHT 

C 

ZQDOT =-6.810E-03*0.5*RHO*L**4 
ZWDOT =-2 . 430E-01*0 . 5*RHO*L**3 
ZQ =-l . 350E-01*0 . 5*RHO*L**3 
ZW =-3.020E-01*0.5*RHO*L**2 
ZDS =-2 . 270E-02*0 . 5*RHO*L**2 
ZDB =-2.270E-02*0.5*RHO*L**2 
C 

MQDOT =-l . 680E-02*0 . 5*RHO*L**5 
MWDOT =-6 . 810E-02*0 .5*RHO*L**4 
MQ =-6.860E-02*0.5*RHO*L**4 
MW = 9 . 860E-02*0 . 5*RHO*L**3 
MDS =-1.113E-02*0.5*RHO*L**3 
MDB = 1. 113E-02*0.5*RHO*L**3 
C 

OPEN ( 1 1 , FILE= ■ THETAl .RES', STATUS= ’ NEW ' ) 

OPEN ( 12,FILE= ' THETA2 .RES ' , STATUS= 'NEW ) 

OPEN ( 13 , FILE= ' THETA3 . RES ' , STATUS= ' NEW ' ) 

OPEN ( 14 , FILE= ' THETA4 .RES ' , STATUS= ' NEW ' ) 

OPEN (21,FILE='DELTA1.RES' , STATUS= ' NEW ' ) 

OPEN (22,FILE='DELTA2.RES' , STATUS= ' NEW ' ) 

C 

SAT =0.4 

SATP= SAT 

SATM=-SAT 

PI =4 .0*ATAN(1.0) 

C 
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c 



( *, 1001 ) 



10 



20 



30 



( * , * ) IVAR 



WRITE 
READ 
GO TO 
WRITE 

READ ( * , * ) 
INCR=IU 

WRITE (*,1003) 

(*,*) 



(10,20,30), 
(*, 1002 ) 

* * 



IVAR 



READ 

WRITE 

READ 

GO TO 

WRITE 

READ 



(*, 1006) 

(*.*) 

15 

( * , 1004 ) 
(*/*) 



INCR=IZG 
WRITE (*,1005) 



READ 

WRITE 

READ 

GO TO 

WRITE 

READ 



(*.*) 



(*,1006) 

(*,*) 

15 

(*,1007) 

(*, *) 
INCR=ITV 
WRITE (*,1003) 
READ 
WRITE 



(*,*) 

(*, 1005) 



READ ( * , * ) 



UMIN,UMAX, lU 



ZG 



TV 



ZGMIN, ZGMAX, IZG 



U 



TV 



TVMIN,TVMAX, ITV 



ZG 



U 



15 DO 1 I=1,INCR 

IF (IVAR.EQ.l) U =UMIN +(UMAX -UMIN ) * ( I-l ) / ( INCR-1 ) 

IF (IVAR.EQ.2) ZG=ZGMIN+(ZGMAX-ZGMIN)*(I-1)/(INCR-1) 

IF (IVAR.EQ.3) TV=TVMIN+(TVMAX-TVMIN) *(!-!)/( INCR-1) 

DV = (MASS-ZWDOT ) * ( lY-MQDOT ) -ZQDOT*MWDOT 

A1 1V= ( ( I Y-MQDOT ) * ZW+ZQDOT*MW ) /DV 

A1 2V= ( ( I Y-MQDOT ) * ( ZQ+MASS ) +ZQDOT*MQ ) /DV 

A13V=- ( ZG-ZB ) * (MASS*XG+ZQDOT ) *WEIGHT/DV 

A2 1 V= ( MWDOT* Z W+ ( MASS-ZWDOT ) *MW ) /DV 

A2 2 V= ( MWDOT* ( Z Q+MAS S ) + ( MAS S - Z WDOT ) * MQ ) / DV 

A23V=- ( ZG-ZB) * (MASS-ZWDOT) *WEIGHT/DV 

B11V=( ( I Y-MQDOT) *ZDS+ZQDOT*MDS)/DV 

B12V= ( ( lY-MQDOT ) *ZDB+ZQDOT*MDB ) /DV 

B2 1V= (MWDOT* Z DS+ ( MASS-ZWDOT ) *MDS ) /DV 

B2 2V= ( MWDOT* Z DB+ ( MASS-ZWDOT ) *MDB ) /DV 

BIV =B11V-B12V 

B2V =B21V-B22V 



OMEGAV=( 10 . 0*U)/(TV*L) 
AD1V=1 .75*OMEGAV 
AD2V=2 . 15*OMEGAV**2 
AD3V=OMEGAV**3 
A2=B1V*U*U 
A3=B2V*U*U 
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D1=-AD1V-(A11V+A22V)*U 

B1=-B2V*U*U 

B2=(B1V*A22V-B2V*A12V) *U**3 
B3=(B2V*A11V-B1V*A21V) *U**3 
D2=AD2V+A23V+(A12V*A21V-A11V*A22V)*U**2 
C1=(B2V*A11V-B1V*A21V)*U**3 
C2=(A23V*B1V-A13V*B2V) *U**2 
D3=AD3V+(A13V*A21V-A11V*A23V)*U 
K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 
K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*B2 ) 
K1V=(D3-C2*K2V)/C1 
K3V=(D1-A2*K2V)/A3 
C 

IF (IVAR.EQ.l) OUT=U 
IF (IVAR.EQ.2) OUT=ZG 
IF (IVAR.EQ.3) OUT=TV 
D3P=(A13V*A21V-A11V*A23V)*U 
XAAA=CBRT(-D3P) 

TVCR=(10. *U)/(XAAA*L) 

IF (TV.LT.TVCR) GO TO 1 
C 

CALL SOLSET( INUM, THSOLS , KIV, Cl , AD3V, SSTH ) 

ICHECK=0 

DO 2 III=1,INUM 

THCH=2 , 0* (THSOLS-0 . 5*PI ) 

CHECK=S IN ( THCH ) * ( D3-AD3V ) /Cl 
IF ( ABS( CHECK ) .GT. SATP) GO TO 2 
WRITE (13,*) OUT, THCH*180.0/PI 
WRITE (14,*) OUT, -THCH*180. 0/PI 
WRITE (21,*) OUT, ABS(CHECK) *180 . 0/PI 
ICHECK=1 

2 CONTINUE 

IF ( ICHECK.EQ. 0) GO TO 3 
GO TO 1 
C 

3 STHETA=SATP*C1/(D3-AD3V) 

SSTH=ASIN(STHETA) 

THETA1= SSTH*180.0/PI 
THETA2=-SSTH*180. 0/PI 
WRITE (11,*) OUT,THETAl 
WRITE (12,*) OUT,THETA2 

WRITE (22,*) OUT, SATP* 180 . 0/PI 
1 CONTINUE 
STOP 
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1006 FORMAT (' ENTER TV) 

1007 FORMAT (' ENTER MIN, MAX, AND INCREMENTS IN TV) 

END 

C 

FUNCTION CBRT(A) 

IF (A.GT.0.0) CBRT= A **(l./3.) 

IF (A.LE.0.0) CBRT=-(-A)**(l./3. ) 

RETURN 

END 

C 

SUBROUTINE SOLSET( L, ANS , KIV, Cl , AD3V, SSTH ) 

REAL KIV 

DIMENSION VF(1,2) 

C 

PI=4.0*ATAN(1.0) 

C 

C FIND FIRST ESTIMATE OF THE SOLUTIONS 

L=0 

VMIN= 0.0 
VMAX=+90.0 
IV=100 

VA=VMIN*PI/I80.0 

VAO=VA 

VO=THETEQ( 1, VA,K1V,C1, AD3V) 

DO 10 1=2, IV 

VA=VMIN+( VMAX-VMIN) * (!-!)/( IV-1 ) 

VA=VA*PI/180.0 

VAN=VA 

VN=THETEQ ( 1 , VA , KIV , Cl , AD3V ) 

VP=VO*VN 

IF (VP.GE.0.0) GO TO 11 
L=L+1 

VF ( L , 1 ) =VAO 
VF ( L , 2 ) =VAN 
GO TO 12 

1 1 VO=VN 
VAO=VAN 

10 CONTINUE 
C 

C EXACT COMPUTATION OF SOLUTIONS VIA NEWTON'S METHOD 

12 E=l.E-5 
IEND=500 

DO 20 J=1,L 

X=(VF( J, 1)+VF(J,2) )/2.0 
F=THETEQ( 1,X,K1V,C1,AD3V) 

FDER=THETEQ ( 2 , X, KIV, Cl , AD3V ) 

DO 30 K=1,IEND 

IF (FDER.EQ.0.0) STOP 1001 

DX=F/FDER 

X1=X-DX 

F=THETEQ ( 1 , XI , KIV, Cl , AD3V ) 
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FDER=THETEQ(2,X1,K1V,C1,AD3V) 

IF (F.EQ. 0 . ) GO TO 35 
A=ABS(X1-X) 

IF (A-E) 35,35,40 
40 X=X1 

30 CONTINUE 
GO TO 20 
35 ANS=X1 
20 CONTINUE 
RETURN 
END 

FUNCTION THETEQ ( K , THETA, K1 V, C 1 , AD3V ) 

REAL KIV 

GO TO (10,20) , K 

10 THETEQ=K1V*C1*THETA+(AD3V-K1V*C1 )*COS( THETA) 
GO TO 50 

20 THETEQ=K1V*C1- (AD3V-K1V*C1 ) *SIN(THETA) 

50 RETURN 
END 



100 



LIST OF REFERENCES 



1. Lienard, D.L. (1990) "Autopilot design for autonomous 
underwater vehicles based on sliding mode control", M.E. 
Thesis, Mechanical Engineering, Naval Postgraduate School, 
Monterey, California. 

2. Sur, J.-N. ( 1989) "Design and investigation of a dive plane 
sliding mode compensator for an autonomous underwater 
vehicle", M.S. Thesis, Naval Postgraduate School, Monterey, 
California . 

3. Chism, S. (1990) "Robust path tracking of autonomous 
underwater vehicles using sliding modes", M.E. Thesis, Naval 
Postgraduate School, Monterey, California. 

4. Hawkinson, T. (1990) "Multiple input sliding mode control 
for autonomous diving and steering of underwater vehicles", 
M.E. Thesis Naval Postgraduate School, Monterey, California. 

5. Kanyama, Y, and Hartman, B.I. (1989) "Smooth local path 
planning for autonomous vehicles". Proceedings, IEEE 
international Conference on Robotics and Automation, 
Scottsdale, Arizona. 



101 



6. Suwandee, P. (1991) "Orientation guidance and control for 
marine vehicles in the horizontal plane", M.S. Thesis, Naval 
Postgraduate School . 

7. Papoulias, F.A. (1991) "Stability considerations of 
guidance and control laws for autonomous underwater vehicles 
in the horizontal plane". Proceedings, 7th International 
Symposium on Unmanned Underwater Submersible Technology, 
Durham, New Hampshire. 

8. Smith, N.S., Crane, J.W., and Summery, D.C. (1978) " SDV 
Simulator hydrodynamic coefficients". Naval Coastal Systems 
Center, Panama City, Flor.^da, Report No. NCSC - TM231 - 78. 



102 



INITIAL DISTRIBUTION LIST 



No . Copies 
2 



1. Defense Technical Information Center 
Cameron Station 
Alexandria, VA 22304-6145 

2. Library, Code 0142 2 

Naval Postgraduate School 

Monterey, CA 93943-5002 

3. Dr. Fotis A. Papoulias , Code 69Pa 2 

Department of Mechanical Engineering 

Naval Postgraduate School 
Monterey, CA 93943 

4. Chairman, Code 69Hy 1 

Department of Mechanical Engineering 

Naval Postgraduate School 
Monterey, CA 93943 

5. Embassy of Greece 5 

Naval Attache 

2228 Massachusetts Ave . , N.W. 

Washington, D.C. 20008 

6. Agelos G. Papasotiriou 3 

Polemi 3 

Patisia 

Athens, Greece 



103 




,1 



Thesis 

P14736 Papasotiriou 

I Three dimensional 

pursuit guidance and 
control of submersible 
vehicles . 



Thesis 

P 147 36 Papasotiriou 
c.l Three dimensional 

pursuit guidance and 
control of submersible 
vehicles. 



